COMMENT ⊗   VALID 00027 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00004 00002	.SBTTL Interpreter	Data structures
C00012 00003	INTINIT, NEWENV, MINTS
C00017 00004	Interpreter itself: INTERP
C00023 00005	  GETARG, GETSCA, GETVEC, GETTRN
C00028 00006	Variable declaration:  MVAR, KVAR
C00032 00007	Stack ops: GTVAL, CHNGE, PUSH
C00038 00008	\ Global reference routines GLBLNK, GLOBSR.
C00043 00009	Flow-of-control: PROC, RETURN
C00049 00010	  ABORT, GODDT, NOOP, FORCHK, JUMP, JUMPC
C00052 00011	  SPAWN, SPROUT, TERMINATE
C00060 00012	Booleans: SLE,SLT,SGE,SGT,SEQ,SNE,AND,LOR,NOT
C00073 00013	return scalars: SADD, SSUB, SMUL, SDIV, SNEG, VDOT, PVDOT, VMAG, SSBRTN
C00081 00014	Vector utilities:  UNITV, CROSV
C00087 00015	TRANS extraction routines: TPOS, TORIEN, TAXIS, TMAGN
C00095 00016	Return vectors: SVMUL, TVMUL, VMAKE, VADD, VSUB
C00100 00017	Return a trans: TMAKE, TVADD, TVSUB, TTMUL, TINVRT, VSAXWR
C00112 00018	Motion:  MOVE, CENTER
C00116 00019		STOP, TABOFS, WHERE, NOTICE
C00122 00020	Condition monitors:  CMMAK 
C00131 00021	  CMENBL, CMDSBL, CMDEST, CMTRIG, CMSKED, CMUNCR
C00140 00022	  CMDONE, CMDUR, CMFORCE, CMSENSE, COMPLY, CMPOFF, VMKFRC, TFRCST
C00156 00023	Events:  SIGNAL, WAITE, PAUSE
C00159 00024	Output routines:  PRINT, VALPRN, VARPRN, TACKVAL, TYPVAL, CVFX
C00164 00025	  BREAK, NOOP, TOPAL
C00166 00026	Initialization psops:  PROG, ENDP, FIXIT (******* CHANGE THESE TOO ********)
C00172 00027	BUGS
C00173 ENDMK
C⊗;
.SBTTL Interpreter	;Data structures

COMMENT ⊗
Register uses in the interpreter:
	R5	used by some routines as the display register
 	R4	points to interpreter status block
 	R3	interpreter stack pointer
 	R2	not used by the main interpreter loop.  Can be munged by
                    any primary interpreter routine.

Each interpreter has a stack which it uses to store pointers to
currently "open" variables.  During the course of a calculation,
operands and temporary result cells will be open in this fashion. 
The "interpreter stack" is pointed to by R3. When a new interpreter
is sprouted, it is given a new stack area. Each interpreter has
certain status information which facilitates transfer of control
between interpreters.  This information is kept in the interpreter
status block, which is always pointed to by R4.  Most important are
the IPC, the Interpreter Program Counter, the ENV, which points to
the local environment, and LEV, which stores the current lexical
level. 

Each procedure has an environment, which is a data area holding
information vital to that procedure.  This includes pointers to all
the variables local to that procedure, and return information.
The environments are administered under the small block allocator
with garbage collection.
 ⊗

	INSTSZ == 20	;Size of an interpreter stack

;Interpreter status block
	II == 0
	XX IPC		;Interpreter program counter. Leave this as first field!
	XX NXTINT 	;Next interpreter in the list.  For GC of the stacks.
	XX STKBAS 	;Location of start of stack area.  Needed
			;for eventual reclamation.
	XX ENV		;Location of local environment
	XX LEV		;Lexical level of current execution
	XX STA		;Status bits for condition codes:  0 means all well.
	XX PDB		;Location of process descriptor block (for reclamation)
	XX EVT		;The event to signal as this interpreter goes away
	XX CMCB		;Pointer to c-m control block if this is a checker or a body
;	XX OLDV		;The "old value" used by changers
;	XX NEWV		;The "new value" used by changers
    .IFNZ ALAID		;Special debugging information
	XX INTNAM	;Name of the interpreter
	XX INTMA1	;  two words
	XX DEBMOD	;The mode bits for debugging.
		ALDSS == 1	;1 => Single step mode
		ASDTE == 2	;1 => Terminate this interpeter
	XX WAKEVT	;Event to wait on during halts
    .ENDC
	ISBS == II/2	;Size (in words) of interpreter status block

;Fixed fields in the environment of each process
	II == 0
	XX SLINK 	;Pointer to environment of next (outer, lower
			;  numbered) block
	XX OLEV		;Old level.  The lexical level of calling process.
	XX OENV		;Old environment, the one for the calling process.
	XX OIPC		;Old IPC.  Program counter for calling process.
	XX LVARS	;First location where pointers to local variables go
	ENVSIZ == II/2	;Size (in words) of environment header
	
;Each environment entry consists of two words. The first gives the accessing
;method (currently only headers) & the data type, while the second contains
;a pointer to the value/header.

;   Data types
	SCLTYP == 1
	VECTYP == 2
	TRNTYP == 3
	EVNTYP == 4
	CMNTYP == 5

;   Access methods
	HDRTYP == 400

;   Mechanism bits.
	YARM == 1
	YHAND == 2
	BARM == 4
	BHAND == 10
	ANARM == YARM + BARM
	AHAND == YHAND + BHAND

;    Servo bits.
	YARMSB == 176000
	YHANDSB == 1000
	BARMSB == 770
	BHANDSB == 4

;   Table offsets for various mechanisms.
	OFYARM == 0
	OFYHAND == 6*2
	OFBARM == 7*2
	OFBHAND == 15*2

;  Environment offsets for the various mechanisms
	YAOFST == 0
	YHOFST == 1
	BAOFST == 2
	BHOFST == 3

;  Environment offsets for the deproach variables
	YDEPROACH == 4
	BDEPROACH == 5

DATA
SYSENV:	0	;SLINK = nil
	0	;OLEV
	0	;OENV
	0	;OIPC
	1$	;LVARS
	HDRTYP+TRNTYP, YARMHD
	HDRTYP+SCLTYP, YHANDH
	HDRTYP+TRNTYP, BARMHD
	HDRTYP+SCLTYP, BHANDH
	TRNTYP, NILTRN		;ydeproach
	TRNTYP, NILTRN		;bdeproach
1$:	0

YARMHD:	0	;Header for YARM
	0	;type = trans device
	YARMSB	;servo bits for coefficient list
	YARM	;mechanism bits
	0	;calc list = nil

YHANDH:	0	;Header for YHAND
	SCDEV	;type = scalar device
	YHANDSB	;servo bits for coefficient list
	YHAND	;mechanism bits
	0	;calc list = nil

BARMHD:	0	;Header for BARM
	0	;type = trans device
	BARMSB	;servo bits for coefficient list
	BARM	;mechanism bits
	0	;calc list = nil

BHANDH:	0	;Header for BHAND
	SCDEV	;type = scalar device
	BHANDSB	;servo bits for coefficient list
	BHAND	;mechanism bits
	0	;calc list = nil

;INTINIT, NEWENV, MINTS
INTEVT:	0		;The event that interlocks references to ISTBLK.
GLBEVT:	0		;The event that interlocks references to GLBTAB.
CODE

INTINIT:	;Initializes the above events
	EVMAK		;Initialize the INTEVT.
	MOV (SP),INTEVT;
	EVSIG 
	EVMAK		;Initialize the GLBEVT.
	MOV (SP),GLBEVT
	EVSIG
	MOV #GLBTAB,GLBEND	;Initialize GLBEND.  This wipes out all globals.
	RTS PC		;Done

MINTS:	;Marking method for interpeters
	PUSH <R2,R3>		;Save R2 & R3
	EVWAIT INTEVT		;Enter critical region
	MOV NXTINT+ISTBLK,R2	;R2 ← LOC[first real interpeter status block]
	BEQ 4$			;If none, then done

	;mark the stack
1$:	MOV STKBAS(R2),R3	;R3 ← LOC[interpreter stack base]
	ADD #2*INSTSZ,R3	;R3 ← LOC[verge of new stack] (INSTSZ is in bytes)
2$:	MOV -(R3),R0		;R0 ← stack entry
	BEQ 3$			;If 0, then end of stack (RF:  this wont work!!)
	JSR PC,MARKQ
	MOV R0,(R3)		;Put it back (compacting may move it)
	BR 2$

	;mark the environment
3$:	MOV ENV(R2),R3		;R3 ← environment
	JSR PC,MRKENV		;Go mark the environment
	
	MOV NXTINT(R2),R2	;R2 ← LOC[next interpreter status block]
	BNE 1$			;Repeat as necessary
4$:	MOV #SYSENV,R3
	JSR PC,MRKENV		;Mark the system variables
	POP <R2,R3>		;Restore R3 & R2
	EVSIG INTEVT
	RTS PC			;Return

MRKENV:	PUSH <R2,LVARS(R3)>	;Save R2 & LOC[first free variable entry]
	ADD #2*ENVSIZ+2,R3	;R3 ← LOC[first variable entry]
1$:	CMP R3,(SP)		;See if we're done
	BHIS 7$
	TSTB 1(R3)		;Check access method
	BNE 3$
	MOV 2(R3),R0		;Direct - R0 ← LOC[value]
	JSR PC,MARKQ		;Mark it
	MOV R0,2(R3)		;Compacting might move it
2$:	TST (R3)+,(R3)+		;R3 ← LOC[next variable entry to mark]
	BR 1$			;Keep going
3$:	MOV 2(R3),R2		;R2 ← LOC[frame header]
	BIT #FTYPE,TYPE(R2)	;See if device
	BEQ 4$			;Don't mark value for devices
	MOV VAL(R2),R0
	JSR PC,MARKQ		;Mark it
	MOV R0,VAL(R2)
4$:	MOV CALCS(R2),R2	;R2 ← list of affixments
	BEQ 2$			;  if any
5$:	BIT #FRAME2+EXPTRN,TYPE(R2)	;See if we should mark the trans
	BNE 6$
	MOV TRANS(R2),R0
	JSR PC,MARKQ		;Mark it
	MOV R0,TRANS(R2)
6$:	MOV (R1),R1		;Deal with the next affixment
	BR 2$
7$:	TST (SP)+		;Clean LVARS off stack
	POP <R2>		;Restore R2
	RTS PC

NEWENV:	;Gets a new environment, returns address in R0.
	FETCH R0	;Get number of variables used in this environment
	ASL R0		;Need 2 words/variable
	ADD #ENVSIZ,R0	;Add in header size
	JSR PC,GTFREE	;Allocate from large blocks
	MOV R0,LVARS(R0)
	ADD #2*ENVSIZ+2,LVARS(R0)	;Initialize where the first free entry
	RTS PC				;  should go

;Interpreter itself: INTERP

	.MACRO MAKEOP CNAME, ANAME	;Compiler name, Address name
	XX	CNAME
	ANAME
	.ENDM
DATA
;The interpreter operation table
INTOPS: MAKEOP XINVALID,INVALID		;Illegal instruction
	.INSRT	INTOPS.PAL[AL,HE]
	INSEND = II	;Marks the end of the instructions
CODE
	.MACRO FETCH foo
	MOV @(R4),foo	;Get next interpreter instruction in foo
	ADD #2,(R4)	;Bump IPC
	.ENDM

	.MACRO BMPIPC
	ADD #2,(R4)	;Bump IPC
	.ENDM

	.MACRO BACKIPC
	SUB #2,(R4)	;Backup IPC
	.ENDM

	.MACRO CCC	;Clear condition code
;	CLR R0		;Clear condition code.  Not used right now.
	.ENDM

	.MACRO SCC	;Set condition code
;	MOV #2,R0	;Set condition code.  Not used right now. (maybe use TST PC)
	.ENDM

INTERP:
	MOV R3,R0	;Save the limit of the interpreter stack for error checking.
	SUB #2*INSTSZ,R0	
	PUSH <R0>
INT1:	CMP R3,(SP)	;Interpreter stack overflow?
	BGE 1$		;No.  Go to next instruction.
	ALERR INTMS3	;Yes.  Complain.
1$:	CLR -2(R3)	;Zero above top of stack - to keep MINTS happy
	FETCH R0	;R0 ← next instruction
	BLE INVALID	;Instruction out of range
	CMP R0,#INSEND	;Is instruction too large?
	BLE INT2	;No.
INVALID:BACKIPC
	ALERR INTMS1	;Yes. complain.
	BMPIPC		;Bump IPC
INT2:	JSR PC,@INTOPS(R0)	;Call the appropriate routine
	BR INT1		;Repeat interpreter loop

DATA
INTMS1:	ASCIE /INTERPRETER INSTRUCTION OUT OF RANGE/
INTMS2:	ASCIE /INTERPRETED INSTRUCTION RETURNED FAILURE/
INTMS3:	ASCIE /INTERPRETER STACK OVERFLOW/
CODE
;  GETARG, GETSCA, GETVEC, GETTRN

GETARG:
COMMENT ⊗
 Arguments:  
   R0=variable name:  high byte is lexical level, low byte is offset.
   R4=pointer to interpreter status block.
 Result:
   R0← pointer to address of desired variable.  
   R1 clobbered.
 This routine returns in R0 a pointer to the location in the current
   environment (or, if necessary, more global environment) which
   points to the variable which is named in R0. ⊗
	PUSH <R2>	;Save R2
	MOV R0,R1
	BIC #177400,R1	;R1 ← Offset desired
	CLRB R0	
	SWAB R0		;R0 ← Lexical level
	BNE 1$
	MOV #SYSENV,R2	;For lexical level 0 use system environment
	BR 3$
1$:	MOV ENV(R4),R2	;R2 ← LOC[local environment]
	SUB LEV(R4),R0	;R0 ← Difference in levels: desired-got
	BEQ 3$		;Diff=0; can use R2 as pointer at right base.
	BHI 4$		;If diff>0, then value inaccessible.
2$:	MOV SLINK(R2),R2;Must go up a level.  R2 ← LOC[more global environment]
	INC R0		;R0 ← New difference in levels
	BNE 2$		;If not yet good, then move up another level
3$:	ASH #2,R1	;Convert offset to environment pointer (each entry = 2 wds)
	ADD #2*ENVSIZ+2,R1	;Add in environment header
	ADD R2,R1	;R1 ← environment + offset = location of desired entry
	MOV R1,R0
	POP <R2>	;Restore R2.
	RTS PC		;Done.
4$:	PUNT GTMS1
DATA
GTMS1:	ASCIE /ATTEMPT TO ACCESS UNAVAILABLE VARIABLE/
CODE

GETSCA:	;Gets place for a scalar result, and places a pointer on
	;the interpreter stack.  Location is returned in R0.  
	;Simple procedure.
    .IFNZ SMALLB
	MOV #SCASPC,R0
	JSR PC,GETSBK	;Allocate from small blocks
 	MOV R0,-(R3)	;Push new value cell pointer on interpreter stack.
	EVSIG SBEVT	;End of critical section
    .IFF
	MOV #2,R0	;Number of words needed
	JSR PC,GTFREE	;R0 ← LOC[new block]
 	MOV R0,-(R3)	;Push new value cell pointer on interpreter stack.
    .ENDC
	RTS PC		;Done

GETVEC:	;Gets place for a vector result, and places a pointer on
	;the interpreter stack.  Location is returned in R0.  
	;Simple procedure.
    .IFNZ SMALLB
	MOV #VCTSPC,R0
	JSR PC,GETSBK	;Allocate from small blocks
 	MOV R0,-(R3)	;Push new value cell pointer on interpreter stack.
	EVSIG SBEVT	;End of critical section
    .IFF
 	MOV #10,R0	;Number of words needed
 	JSR PC,GTFREE	;R0 ← LOC[new block]
	MOV R0,-(R3)	;Push new value cell pointer on interpreter stack.
    .ENDC
	RTS PC		;Done

GETTRN:	;Gets place for a trans result, and places a pointer on
	;the interpreter stack.  Location is returned in R0.  
	;Simple procedure.
    .IFNZ SMALLB
	MOV #TRNSPC,R0
	JSR PC,GETSBK	;Allocate from small blocks
 	MOV R0,-(R3)	;Push new value cell pointer on interpreter stack.
	EVSIG SBEVT	;End of critical section
    .IFF
 	MOV #40,R0	;Number of words needed
	JSR PC,GTFREE	;R0 ← LOC[new block]
	MOV R0,-(R3)	;Push new value cell pointer on interpreter stack.
    .ENDC
	RTS PC		;Done

;Variable declaration:  MVAR, KVAR;

MVAR:	;Interpreter routine

COMMENT ⊗ A list of arguments, each of which is a type/count pair. This list
is terminated by a zero entry.  For each data type entries in the environment
are created for the specified number of variables. Algebraic variables (scalar,
vector & trans) initially have no value. Events get created and their identifiers
are stored in the environment. For cmons a new condition monitor of the indicated
type is created and a pointer to its control block is placed in the environment.
Frame headers are created by AFFIX. ⊗

	MOV ENV(R4),R2		;R2 ← LOC[current environment]
	MOV LVARS(R2),R2	;R2 ← LOC[first free entry in environment]
1$:	FETCH R1		;R1 ← data type of variables to make
	TST R1
	BNE 2$			;  if any
	MOV ENV(R4),R0		;R0 ← LOC[environment]
	MOV R2,LVARS(R0)	;Update first free variable entry
	CCC
	RTS PC			;(who says returns should go at the end?)
2$:	FETCH R0		;Get count of # of this type of variable to make
	CMP R1,#EVNTYP		;See what data type they should be
	BEQ 4$
	BGT 5$
3$:	MOV R1,(R2)+		;For algebraic just stick data type in place
	CLR (R2)+		;  & zero the value pointer
	SOB R0,3$		;  for each one
	BR 1$
4$:	MOV R1,(R2)+		;Set data type to event
	EVMAK			;Make a new event
	MOV (SP)+,(R2)+		;  & store its identifier in the environment
	SOB R0,4$		;  for each one
	BR 1$
5$:	JSR PC,CMMAK		;Make the new cmons & store them in environment
	BR 1$


KVAR:	;Interpreter routine

COMMENT ⊗ Given the number of variables to kill. The last ones in the
current environment are destroyed. For each frame an attempt is made to
validate any dependents first. ⊗

	FETCH R1		;Get # of variables to kill
	MOV ENV(R4),R2		;R2 ← LOC[environment]
	MOV LVARS(R2),R2	;R2 ← LOC[first free variable entry]
1$:	SUB #4,R2		;R2 ← LOC[variable to kill]
	TSTB 1(R2)		;Test access method
	BEQ 2$
	TST (R2)+
	JSR PC,KFRAME		;Kill the frame & its affixments
	BR 4$
2$:	CMP (R2)+,#EVNTYP	;What type is it?
	BLT 4$			;Algebraic types are easy
	BGT 3$
	EVKIL (R2)		;Kill the event
	BR 4$
3$:	JSR PC,CMDEST		;Kill the cmon
4$:	CLR (R2)		;Zero the pointer field
	CLR -(R2)		;Zero the type field
	SOB R1,1$		;Kill all that we were asked to
	MOV ENV(R4),R0		;R0 ← LOC[environment]
	MOV R2,LVARS(R0)	;Update first free variable entry
	CCC		;Clear condition code
	RTS PC		;Done

;Stack ops: GTVAL, CHNGE, PUSH

GTVAL:
COMMENT ⊗ The argument is a level-offset pair.  The variable
referenced by that pair is examined and a pointer to its value cell
is placed on the stack. ⊗
	FETCH R0	;Pick up level-offset name of argument
	JSR PC,GETARG	;R0 ← LOC[variable environment entry]
	TSTB 1(R0)	;Check access method
	BNE 1$
	MOV 2(R0),-(R3)	;Direct - push value pointer on stack
	BNE 4$		;If it had a value all done
	CMP #TRNTYP,(R0)
	BEQ 2$		;Use niltrans for default
	MOV #NILVEC,(R3);Use vector/scalar zero
	BR 3$
1$:	MOV 2(R0),R0	;R0 ← LOC[frame header]
	JSR PC,NOCMP	;Don't compact for a bit
	CALL GETVAL,<R0>;R0 ← value
	MOV R0,-(R3)	;Push value on interpreter stack.
	JSR PC,YESCMP	;OK to compact now
	TST (R3)	;Check that we got a valid value
	BNE 4$
2$:	MOV #NILTRN,(R3);If not use the niltrans
3$:	ALERR GTVMES	;  & complain
4$:	CCC		;Clear condition code.
	RTS PC		;Done
DATA
GTVMES:	ASCIE </NO VALUE FOR VARIABLE - USING DEFAULT./>
CODE

CHNGE:
COMMENT ⊗ Pops the value from top of stack into the variable specified
by the level-offset pair given in the argument.  ⊗
	FETCH R0	;Pick up level-offset name of argument
	JSR PC,GETARG	;R0 ← LOC[variable environment entry]
	TSTB 1(R0)	;Chech access method
	BNE 1$
	MOV (R3)+,2(R0)	;Direct - store value pointer away
	BR 2$
1$:	MOV 2(R0),R0	;R0 ← LOC[Desired frame header]
	JSR PC,NOCMP	;Don't compact for a bit
	CALL CHANGE,<R0,(R3)>
	JSR PC,YESCMP	;OK to compact now
	TST (R3)+	;Pop stack
2$:	CCC		;Clear condition code.
	RTS PC		;Done


PUSH:	FETCH -(R3)	;Put argument directly on stack
	CCC		;Clear condition code.
	RTS PC		;Done

COMMENT \ ;Global reference routines GLBLNK, GLOBSR.

GLBLNK:	;Interpreter routine
COMMENT ⊗ Expects two arguments at the IPC, a level-offset, and two
words of a Rad50 name.  Makes sure that this global is linked in to
the environment at the given level-offset.  If not, a search is
made for it, and the result is put in the environment.
⊗
	FETCH R0	;R0 ← level-offset
	JSR PC,GETARG	;R0 ← LOC[environment cell]
	TST (R0)	;Graph node yet?
	BEQ 2$		;No, must search for it
1$:	BMPIPC		;Bump IPC past the Rad50 name
	BMPIPC		;Bump IPC past the Rad50 name
	RTS PC		;Done
2$:	MOV R0,R2	;R2 ← LOC[environment cell]
	MOV IPC(R4),R0	;R0 ← LOC[Rad50 representation]
	JSR PC,GLOBSR	;R0 ← LOC[new or old graph node]
	MOV R0,(R2)	;Stow LOC[graph node] in the environment cell
	BR 1$		;Ready to return
DATA
MAXGLB == 10		;Maximum number of globals allowed
GLBTAB:	.BLKW 3*MAXGLB	;Three words per global:  2 of Rad50, one
				;pointer to the graph node.
				;To be searched linearly.
GLBLIM:	.BLKW 3		;Overflow place for GLBTAB
GLBEND:	.BLKW 1		;Points to next free place in GLBTAB
CODE
GLOBSR:
COMMENT ⊗ R0 = LOC[two words of Rad50].  Tries to find the
appropriate graph node using the GLBTAB, and if it fails, makes a new
graph node and inserts it in the GLBTAB.  In any case, returns R0 ←
LOC[new or old graph node].  ⊗
	EVWAIT GLBEVT	;Critical region starts here
	MOV GLBEND,R1	;R1 ← LOC[next free place in GLBTAB]
	MOV (R0),(R1)+	;Put the word sought at next free place
	MOV 2(R0),(R1)+	;
	CLR (R1)	; with a 0 for a graph node pointer.
	MOV #GLBTAB,R1	;R1 ← LOC[start of GLBTAB]
1$:	CMP (R0),(R1)	;MATCH?
	BNE 2$		;No.
	CMP 2(R0),2(R1)	;Second word match?
	BEQ 3$		;Yes.
2$:	ADD #6,R1	;
	BR  1$		;Try again.
3$:	MOV 4(R1),R0	;R0 ← LOC[graph node]
	BNE 5$		;If it is not zero, we are done
	ADD #6,GLBEND	;Move the end of the table down one entry
	CMP GLBEND,#GLBLIM	;Too far?
	BLT 4$		;No
	ALERR GLOBMS	;Yes
4$:	MOV R1,-(SP)	;Save place in GLBTAB
	CLR R0		;New graph node should have no value cell.
	JSR PC,MAKEVN	;R0 ← LOC[a new variable node]
	MOV (SP)+,R1	;Restore place in GLBTAB
	MOV R0,4(R1)	;store LOC[new graph node] in GLBTAB
5$:	EVSIG GLBEVT	;Critical region ends here
	RTS PC		;Done
DATA
GLOBMS:	ASCIE </TOO MANY GLOBALS/>
CODE
\
;Flow-of-control: PROC, RETURN

PROC:
RETURN:
COMMENT \  Commented out for now
;Procedure call.  Arguments: 
;	Destination.
;	List of variables which are to be inserted in appropriate 
;	  locations in the local storage of procedure.  These are
;	  in the format variable (ie level-offset pair), new offset
;	  (right justified in the second word).
;	  There is a zero word to finish these.
;At the destination address can be found:
	II == 0
	XX FSLGTH	;Number of words to get from free storage 
			;for local variable pointers
	XX PLEV		;Lexical level of procedure
	DSLGTH == II	;Number of words before code starts
;Value parameters should have first been copied first into local temps
;  (which have been arranged by the compiler), and then the temps are
;  passed by reference.  Eventual problem: to know which variables to
;  really kill as the procedure is exited. 

	MOV @IPC(R4),R2	;R2 ← LOC[destination]
	BMPIPC		;Bump IPC
	MOV FSLGTH(R2),R0	;R0 ← Number of words to get.
	JSR PC,GTFREE	;R0 ← LOC[block with that number of words]

      ;initialize pointer to lexical level:
	MOV PLEV(R2),R1	;R1 ← Lexical level of procedure
	MOV ENV(R4),R2	;R2 ← LOC[current environment]
	SUB LEV(R4),R1	;R1 ← Difference in levels: desired-got
	BEQ 2$		;Diff=0; can use R2 as pointer at right environment.
1$:	MOV SLINK(R2),R2;No, must go up a level.  R2 ← LOC[base of upper area]
	INC R1		;R1 ← New difference in levels
	BNE 1$		;If not yet good, then move up another level
2$:	MOV R2,SLINK(R0);SLINK[new environment] ← correct global environment

      ;Put copies of local variables in new area
	MOV R0,-(SP)	;Stack LOC[new environment]
	MOV @IPC(R4),R0	;R0 ← level-offset pair for an argument
	BEQ 4$		;If there are no more, go to next phase
3$:	BMPIPC		;Else bump IPC
	JSR PC,GETARG	;R0 ← LOC[LOC[graph node]]
	MOV @IPC(R4),R1	;R1 ← offset in new block
	BMPIPC		;Bump IPC
	ADD (SP),R1	;R1 ← LOC[place in new environment to put pointer]
	MOV (R0),(R1)	;new environment gets pointer to LOC[argument graph node]
	MOV @IPC(R4),R0	;R0 ← level-offset pair for an argument
	BNE 3$		;If there are more, go back and treat them
4$:	BMPIPC		;Bump IPC one last time

      ;Save the old context in the new area
	MOV (SP)+,R1	;R1 ← LOC[new environment]
	MOV LEV(R4),OLEV(R1)	;Store the old level
	MOV ENV(R4),OENV(R1)	;Store the old environment location
	MOV IPC(R4),OIPC(R1)	;Store the return address

      ;Set up the new context for procedure
	MOV PLEV(R2),LEV(R4)	;New lexical level
	MOV R1,ENV(R4)	;New environment location
	ADD #DSLGTH,R2	;R2 ← Place where execution should begin
	MOV R2,IPC(R4)	;New program counter
	CCC		;Clear condition code.
	RTS PC		;Done


RETURN:
;Returns from a procedure call to calling program. Since variables are
;passed by reference, it is not necessary to do any copying of values.
;All that is needed is to restore the context of the caller and to
;discard the display.
	MOV ENV(R4),R0	;R0 ← LOC[current environment]
	MOV OLEV(R0),LEV(R4)	;Restore the old lexical level
	MOV OENV(R0),ENV(R4)	;Restore the old environment
	MOV OIPC(R0),IPC(R4)	;Restore the IPC
	JSR PC,RLFREE	;Release storage of old display
	CCC		;Clear condition code.
	RTS PC		;Done
\
;  ABORT, GODDT, NOOP, FORCHK, JUMP, JUMPC

ABORT:
;Aborts current motions
;This should be cleaned up sometime.
	MOV	#16,R1	;First stop all devices - 2 arms (6 joints/arm) & 2 hands
	MOV	LDVCPTR,R0	;R0 ← LOC[table of device pointers]
1$:	MOV	(R0)+,R2	;R2 ← device block
	BEQ	2$		;If any
	BIS	#100000,@0(R2)	;Stop this device.
2$:	SOB	R1,1$		;Repeat	till all devices stopped
	;SLEEP	#144		;Should pause for a bit (1/10 sec) here but...
				;  if anything gets printed no problem
	CCC			;Clear the condition codes
	RTS	PC		; & Return

GODDT:	BPT			;break to DDT
NOOP:	CCC			;Clear the condition codes
	RTS	PC		; and Return

FORCHK:	
;Assume that the stack has, from surface in, the increment, the
;  final value, and the control variable's value, all of which are
;  scalar values.  If (FINAL-CONVAR)*(INCREMENT) ≥ 0 then this is a
;  no-op; otherwise, jump to the destination. 
;Arguments:  destination.
	LDF @2(R3),AC0	;AC0 ← final value
	SUBF @4(R3),AC0	;AC0 ← final - current
	MULF @(R3),AC0	;AC0 ← (final - current)*increment
	FETCH R0	;R0 ← destination
	CFCC
	BGE 1$		;Shall this be a no-op?
	MOV R0,IPC(R4)	;No; set new IPC.
1$:	CLR R0
	RTS PC		;Done

JUMP:
;Takes one argument: the new address.
	MOV @IPC(R4),IPC(R4)
	CCC		;Clear condition code.
	RTS PC		;Done

JUMPC:	;Interpreter routine
COMMENT ⊗ Takes one argument: the destination address. 
The condition queries the top of the stack and pops it, assuming it
to be a scalar.  The interpreter jumps to the destination address if
the value of the scalar is false (0). rewritten 9-14-76 by arg ⊗
	LDF	@(R3)+,AC0	;Get value of boolean
	CFCC			;copy condition codes
	BEQ	1$		;if false succeed - take branch
	BMPIPC			;skip over address
	RTS	PC		; & return
1$:	MOV	@IPC(R4),IPC(R4); branch
	RTS	PC		; & return

;  SPAWN, SPROUT, TERMINATE


SPAWN:	;Utility routine

COMMENT ⊗ Takes two arguments: In R0, the IPC of the interpreter to
spawn, and in R1, the event (if any) to put in EVT of the new
interpreter.  The inferior will have the same environment as the
superior.  Creates an interpreter status block, stack, process
descriptor, and is ready for a SCHEDU when it returns the process
descriptor in R0. ⊗

	PUSH <R1,R0>		;Save the EVT & the new IPC
	MOV #ISBS,R0		;R0 ← Size (in words) of an interpreter status block
	JSR PC,GTFREE		;R0 ← LOC[new interpreter status block]
	POP <IPC(R0)>		;new IPC ← first argument
	MOV ENV(R4),ENV(R0)	;new ENV ← old ENV
	MOV LEV(R4),LEV(R0)	;new LEV ← old LEV
    .IFNZ ALAID
	MOV DEBMOD(R4),DEBMOD(R0)	;new DEBMOD ← old DEBMOD
    .ENDC
	EVWAIT INTEVT		;Interlock sensitive operation.
	MOV #NXTINT+ISTBLK,R1	;Link into the interpreter list.
	MOV (R1),NXTINT(R0)
	MOV R0,(R1)
	EVSIG INTEVT		;End of interlock
	POP <EVT(R0)>		;new EVT ← second argument.
	PUSH <R0>		;Save LOC[new interpreter status block]
	MOV #INSTSZ,R0		;R0 ← Size needed for an interpreter stack
	JSR PC,GTFREE		;R0 ← LOC[new interpreter stack]
	POP <R1>		;R1 ← LOC[new interpreter status block]
	MOV R0,STKBAS(R1)	;Store away new stack base
	ADD #2*INSTSZ,R0	;R0 ← LOC[top of new stack] (INSTSZ is in bytes)
	PUSH <R1,R0>		;Save R1 & R0
	MOV #210,R0		;Room for process descriptor
	JSR PC,GTFREE		;R0 ← LOC[new process descriptor]
	MOV #UFPUSE+UGRSAV+2,PDBSTA(R0)	;Use floating point,saved registers, pri=1
	POP <PDBR3(R0),R1>	;Store away new interp stack pointer (reg 3)
				;R1 ← LOC[new ISB]
	MOV R0,PDB(R1)		;Store away LOC[PDB] in new ISB
	MOV R1,PDBR4(R0)	;Store away LOC[ISB] in reg 4 of PDB
	MOV R0,USKMIN(R0)	;Set up min pointer for SP
	ADD #UFEC+36,USKMIN(R0)
	MOV R0,USKMAX(R0)	;Set up max pointer for SP
	ADD #420,USKMAX(R0)
	MOV #144040,UPSW(R0)	;Set up psw
	MOV PDB(R4),R1		;Use same UIMAP that we are using.
	MOV UIMAP(R1),UIMAP(R0)

	RTS PC		;Done

; These are the appropriate scheduling commands:
;    SCHEDU R0,#INTERP,#USRDM,#2   ;Cause the new process to be started, suspended
;    FORK R0,#INTERP,#USRDM	   ;Cause the new process to be started.

SPROUT:	;Interpreter routine

COMMENT ⊗ Arguments: One address in pseudo-code for each of the
several forks starting up, followed by a 0 word.  This is to be used
only for cobegins, not for servos.  Each new interpreter is given an
interpreter status block and is then scheduled.  As each terminates,
it signals its defining event.  Since each of these has the same
event, the current interpreter need only wait until they all happen.
⊗

	PUSH <R3>	;Save R3.  Caution:  cannot use interpreter stack now.
	CLR R3		;R3 is the count of how many inferiors to spawn.
	EVMAK		;-(SP) ← Event identifier for communication with infs.
1$:	FETCH R0	;R0 ← next argument (IPC)
	TST R0
	BEQ 2$		;If zero, then we have spawned all the inferiors.
	INC R3		;Count it.
	MOV (SP),R1	;R1 ← event for the inferior EVT
	JSR PC,SPAWN
	MOV R0,R2	;R2 ← new process control block 
	;Set up the new environment
	JSR PC,NEWENV	;R0 ← LOC[new environment]
	MOV ENV(R4),SLINK(R0)	;Not necessary to set up OLEV, etc.
	MOV PDBR4(R2),R1
	MOV R0,ENV(R1)
   .IFNZ SMALLB
	EVSIG SBEVT	;End of critical section - value stored
   .ENDC
	INC LEV(R1)
	SCHEDU R2,#INTERP,#USRDM,#2 ;Cause the new process to be started, suspended
	BR  1$		;Go handle the next inferior.
2$:	DEC R3		;Another wait to be done?
	BMI 3$		;No, we are finished.
	EVWAIT (SP)	;Wait for an inferior to come back.
	BCC 2$		;If all well, wait for the next one.
	ALERR SPRMES	;The event was killed!
3$:	EVKIL (SP)+	;Kill the event now, remove from stack
	POP <R3>	;Restore R3
	CCC		;Clear condition code.
	RTS PC		;Done
DATA
SPRMES: ASCIE /BAD RETURN FROM INFERIOR/
CODE


TERMINATE:	
COMMENT ⊗ Interpreter routine, sometimes jumped to from other
interpreter routines.  End this interpreter.  ⊗
	MOV EVT(R4),R0		;R0 ← event to announce imminent demise
	BEQ 1$			;If there is one
	EVSIG R0		;Announce that we are about to disappear.
1$:	MOV STKBAS(R4),R0	;Reclaim interpreter stack
	JSR PC,RLFREE
	MOV ENV(R4),R0		;Reclaim this environment
	JSR PC,RLFREE
	PUSH <PDB(R4)>		;Save LOC[this PDB]
	MOV R4,R0		;Reclaim Interpreter Status Block
	JSR PC,RLFREE
	EVWAIT INTEVT		;Enter critical region.
	MOV #ISTBLK,R0	;The following unlinks this interpreter from the chain.
2$:	MOV R0,R1
	MOV NXTINT(R1),R0
	CMP R0,R4		;Have we found ours yet?
	BNE 2$
	MOV NXTINT(R4),NXTINT(R1)	; Yes. rechain.
	EVSIG INTEVT		;Leave critical region.
	POP <R0>		;Reclaim process control block (should be safe now)
	CMP R0,#FREEST		;Make sure that it points into free storage.
	BLE 3$			; (it may be statically allocated)
	CMP R0,#FREEND
	BGE 3$
	JSR PC,RLFREE
3$:	DISMIS			;Go away
;Booleans: SLE,SLT,SGE,SGT,SEQ,SNE,AND,LOR,NOT

COMP:	;auxiliary function used by SLE,SLT,SGE,SGT,SEQ,SNE
	LDF	@(R3)+,AC0	;Get first arg
	CMPF	@(R3)+,AC0	;Compare it with second arg (1st-2nd)
	JSR 	PC,NOCMP	;Don't compact for a bit
	JSR	PC, GETSCA	;R0 ← -(R3) ← LOC[new_scalar]
	MOV	ONE,(R0)+	;assume true (1.0)
	CLR	(R0)
	JSR 	PC,YESCMP	;OK to compact now
	CFCC			;copy condition flags from compare
	RTS	PC		; & Return

SLT:	JSR	PC,COMP		;compare the args
	BLT	1$		;if true then done
	CLR	@(R3)		;else set answer to false (0)
1$:	RTS	PC		; & return

SLE:	JSR	PC,COMP		;compare the args
	BLE	1$		;if true then done
	CLR	@(R3)		;else set answer to false (0)
1$:	RTS	PC		; & return

SGT:	JSR	PC,COMP		;compare the args
	BGT	1$		;if true then done
	CLR	@(R3)		;else set answer to false (0)
1$:	RTS	PC		; & return

SGE:	JSR	PC,COMP		;compare the args
	BGE	1$		;if true then done
	CLR	@(R3)		;else set answer to false (0)
1$:	RTS	PC		; & return

SEQ:	JSR	PC,COMP		;compare the args
	BEQ	1$		;if true then done
	CLR	@(R3)		;else set answer to false (0)
1$:	RTS	PC		; & return

SNE:	JSR	PC,COMP		;compare the args
	BNE	1$		;if true then done
	CLR	@(R3)		;else set answer to false (0)
1$:	RTS	PC		; & return

AND:	LDF	@(R3)+,AC0	;Get first arg
	LDF	@(R3)+,AC1	;Get second arg (and set condition flags)
	JSR 	PC,NOCMP	;Don't compact for a bit
	JSR	PC, GETSCA	;R0 ← -(R3) ← LOC[new_scalar]
	CLR	(R0)+		;assume false (0)
	CLR	(R0)
	JSR 	PC,YESCMP	;OK to compact now
	CFCC			;copy condition flags for 2nd arg
	BEQ	1$		;if it's false return false
	TSTF	AC0		;else look at 1st arg
	CFCC
	BEQ	1$
	MOV	ONE,@(R3)	;if both args are true return true (1.0)
1$:	RTS	PC		; Return

LOR:	LDF	@(R3)+,AC0	;Get first arg
	LDF	@(R3)+,AC1	;Get second arg (and set condition flags)
	JSR 	PC,NOCMP	;Don't compact for a bit
	JSR	PC, GETSCA	;R0 ← -(R3) ← LOC[new_scalar]
	MOV	ONE,(R0)+	;assume true (1.0)
	CLR	(R0)
	JSR 	PC,YESCMP	;OK to compact now
	CFCC			;copy condition flags from compare
	BNE	1$		;if it's true return true
	TSTF	AC0		;else look at 1st arg
	CFCC
	BNE	1$
	CLR	@(R3)		;if both args are false return false (0)
1$:	RTS	PC		; Return

NOT:	LDF	@(R3)+,AC0	;Get arg (and set condition flags)
	JSR 	PC,NOCMP	;Don't compact for a bit
	JSR	PC, GETSCA	;R0 ← -(R3) ← LOC[new_scalar]
	CLR	(R0)+		;assume false (0)
	CLR	(R0)
	JSR 	PC,YESCMP	;OK to compact now
	CFCC			;copy condition flags for arg
	BNE	1$		;if it's false return true
	MOV	ONE,@(R3)	; else return true
1$:	RTS	PC		; Return

;return scalars: SADD, SSUB, SMUL, SDIV, SNEG, VDOT, PVDOT, VMAG, SSBRTN

COMMENT ⊗ All timings are averages of 1000 runs.  They take into
account the cost of the RTS but not the JSR.  It is assumed that
GETSCA and GETVEC take no time.  All routines on this page are
interpreter routines.  ⊗

;30 microseconds
SADD:	;Scalar ← Scalar + Scalar
	LDF @(R3)+,AC0	;AC0 ← arg 2
	ADDF @(R3)+,AC0	;AC0 ← arg2 + arg1
	JSR PC,GETSCA	;R0 ← -(R3) ← LOC[new scalar block]
	STF AC0,@(R3)	;Store result
	CCC		;Clear condition code.
	RTS PC		;Done

SSUB:	;Scalar ← Scalar - Scalar
	LDF @2(R3),AC0	;AC0 ← arg 1
	SUBF @(R3)+,AC0	;AC0 ← arg1 - arg2
	TST (R3)+	;Move past first argument
	JSR PC,GETSCA	;R0 ← -(R3) ← LOC[new scalar block]
	STF AC0,@(R3)	;Store result
	CCC		;Clear condition code.
	RTS PC		;Done

;30 microseconds
SMUL:	;Scalar ← scalar * scalar
	LDF @(R3)+,AC0	;AC0 ← arg 2
	MULF @(R3)+,AC0	;AC0 ← arg2 * arg1
	JSR PC,GETSCA	;R0 ← -(R3) ← LOC[new scalar block]
	STF AC0,@(R3)	;Store result
	CCC		;Clear condition code.
	RTS PC		;Done

;33 microseconds
SDIV:	;Scalar ← Scalar / Scalar
	LDF @(R3)+,AC1	;AC1 ← arg 2
	LDF @(R3)+,AC0	;AC0 ← arg 1
	DIVF AC1,AC0	;AC0 ← arg1 / arg2
	JSR PC,GETSCA	;R0 ← -(R3) ← LOC[new scalar block]
	STF AC0,@(R3)	;Store result
	CCC		;Clear condition code.
	RTS PC		;Done

;26 microseconds
SNEG:	;Scalar ← -Scalar
	LDF @(R3)+,AC0	;AC0 ← arg
	NEGF AC0	;AC0 ← -arg
	JSR PC,GETSCA	;R0 ← -(R3) ← LOC[new scalar block]
	STF AC0,@(R3)	;Store result
	CCC		;Clear condition code.
	RTS PC		;Done

;96 -- 116 microseconds
VDOT:	;Scalar ← Vector dot Vector
	;S ← (X1X2 + Y1Y2 + Z1Z2) / W1W2
	PUSH <R2>	;Save R2.
	JSR PC,NOCMP	;Don't compact for a bit
	MOV (R3)+,R1	;R1 ← LOC[arg 2]
	MOV (R3)+,R0	;R0 ← LOC[arg 1]
	CLRF AC0	;AC0 ← 0.  Running total
	MOV #3,R2	;R2 ← 3:  Length of vector
1$:	LDF (R0)+,AC1	;Form sum of products of first 3 terms
	MULF (R1)+,AC1	;
	ADDF AC1,AC0	;
	SOB R2,1$	;Loop until all 3 fields done.
	DIVF (R0),AC0	;Divide by W1
	DIVF (R1),AC0	;Divide by W2.  AC0 now has answer.
	JSR PC,YESCMP	;OK to compact now
	JSR PC,GETSCA	;R0 ← -(R3) ← LOC[new scalar block]
	STF AC0,@(R3)	;Store result
	POP <R2>	;Restore R2
	CCC		;Clear condition code.
	RTS PC		;Done

COMMENT ⊗
;103 -- 116 microseconds
PVDOT:	;Scalar ← Plane dot Vector
	;S ← X1X2 + Y1Y2 + Z1Z2 + W1W2
	MOV R2,-(SP)	;Save R2.
	JSR PC,NOCMP	;Don't compact for a bit
	MOV (R3)+,R1	;R1 ← LOC[arg 2]
	MOV (R3)+,R0	;R0 ← LOC[arg 1]
	CLRF AC0	;AC0 ← 0.  Running total
	MOV #4,R2	;R2 ← 4:  Length of vector and weight
1$:	LDF (R0)+,AC1	;Form sum of products of all 4 terms
	MULF (R1)+,AC1	;
	ADDF AC1,AC0	;
	SOB R2,1$	;Loop until all 3 fields done.
	JSR PC,YESCMP	;OK to compact now
	JSR PC,GETSCA	;R0 ← -(R3) ← LOC[new scalar block]
	STF AC0,@(R3)	;Store result
	MOV (SP)+,R2	;Restore R2
	CCC		;Clear condition code.
	RTS PC		;Done
⊗
;199 -- 207 microseconds
VMAGN:	;Scalar ← Norm (vector)
	;S ← SQRT(XX + YY+ ZZ) / W
	JSR PC,NOCMP	;Don't compact for a bit
	MOV (R3)+,R1	;R1 ← LOC[arg]
	LDF (R1)+,AC0	;AC0 ← X
	MULF AC0,AC0	;AC0 ← XX
	LDF (R1)+,AC1	;AC1 ← Y
	MULF AC1,AC1	;AC1 ← YY
	ADDF AC1,AC0	;AC0 ← XX + YY
	LDF (R1)+,AC1	;AC1 ← Z
	MULF AC1,AC1	;AC1 ← ZZ
	ADDF AC1,AC0	;AC0 ← XX + YY + ZZ
	MOV R1,-(SP)	;Push LOC[W] onto system stack, to save across SQRTF
	JSR PC,@LSQRTF	;AC0 ← SQRT(XX + YY + ZZ)
	DIVF @(SP)+,AC0	;AC0 ← AC0 / W
	JSR PC,YESCMP	;OK to compact now
	JSR PC,GETSCA	;R0 ← -(R3) ← LOC[new scalar block]
	STF AC0,@(R3)	;Store answer
	CCC		;Clear condition code.
	RTS PC		;Done

SSBRTN:	;Call a routine.
	LDF @(R3)+,AC0	;AC0 ← arg
	FETCH R0	;R0 ← which routine (a small number)
	ASL R0		;Double (words → bytes)
	BLE 1$		;Too small.
	CMP R0,#SBLSIZ	;Too large?
	BGE 1$		;Yes
	JSR PC,@SBRLST(R0)	;Call a routine.  AC0 ← answer.
	JSR PC,GETSCA	;R0 ← -(R3) ← LOC[new scalar block]
	STF AC0,@(R3)	;Store answer
	CCC		;Clear condition code.
	RTS PC		;Done
1$:	ALERR SSBRMS	;Complain
	SCC		;Set condition code
	RTS PC		;Done
DATA
SSBRMS:	ASCIE </NO SUCH SUBROUTINE/>

SBRLST:	;List of legal subroutines
	0		;Illegal
	SQRT		;The only one right now. #1
	SIN		;#2
	COS		;#3
	ASIN		;#4
	ACOS		;#5
	ATAN2		;#6
SBLSIZ == .-SBRLST	;The size of the list (bytes)

CODE
SQRT:	JMP @LSQRTF	;Let it do the returning
SIN:	JMP @LSNCSD	;Let it do the returning
COS:	JSR PC,@LSNCSD
	LDF AC1,AC0
	RTS PC
ASIN:	JMP @LASIN	;Let it do the returning
ACOS:	JMP @LACOS	;Let it do the returning
ATAN2:	LDF @(R3)+,AC1	;Get second argument for atan2(#1,#2)
	JMP @LATAN2

;Vector utilities:  UNITV, CROSV

;281 -- 286 microseconds  
UNITV:	;Vector ← V / Norm(V)
	;S ← SQRT(XX + YY+ ZZ)
	JSR PC,NOCMP	;Don't compact for a bit
	MOV (R3),R1	;R1 ← LOC[arg]
	LDF (R1)+,AC0	;AC0 ← X
	MULF AC0,AC0	;AC0 ← XX
	LDF (R1)+,AC1	;AC1 ← Y
	MULF AC1,AC1	;AC1 ← YY
	ADDF AC1,AC0	;AC0 ← XX + YY
	LDF (R1)+,AC1	;AC1 ← Z
	MULF AC1,AC1	;AC1 ← ZZ
	ADDF AC1,AC0	;AC0 ← XX + YY + ZZ
	JSR PC,@LSQRTF	;AC0 ← SQRT(XX + YY + ZZ)
	JSR PC,GETVEC	;R0 ← -(R3) ← LOC[new vector block]
	MOV 2(R3),R1	;R1 ← LOC[old vector]
	MOV #3,R2	;R2 ← count of fields
1$:	LDF (R1)+,AC1	;AC1 ← field of vector
	DIVF AC0,AC1	;divide by norm
	STF AC1,(R0)+	;Store result
	SOB R2,1$	;Loop until done
	MOV ONE,(R0)+	;Set W to 1
	CLR (R0)	;   (two words long)
	MOV (R3)+,(R3)	;Fix-up stack
	JSR PC,YESCMP	;OK to compact now
	CCC		;Clear condition code
	RTS PC		;Done

;172 -- 184 microseconds  
CROSV:	;Vector ← Vector cross Vector
	;X ← Y1Z2 - Y2Z1
	;Y ← X2Z1 - X1Z2
	;Z ← X1Y2 - X2Y1
	;W ← W1W2
	;AC0, 1, 2, 3, 4, 5 are garbaged by this routine.
	JSR PC,NOCMP	;Don't compact for a bit
	JSR PC,GETVEC	;R0 ← -(R3) ← LOC[new vector block]
	MOV 2(R3),R2	;R2 ← LOC[arg 2]
	MOV 4(R3),R1	;R1 ← LOC[arg 1].  Must not pop R3 stack yet!
	LDF 14(R1),AC0	;AC0 ← W1
	MULF 14(R2),AC0	;AC0 ← W1W2
	STF AC0,14(R0)	;Store AC0 → W
	LDF 4(R1),AC0	;AC0 ← Y1
	LDF (R2),AC1	;AC1 ← X2
	LDF 4(R2),AC2	;AC2 ← Y2
	LDF (R1),AC3	;AC3 ← X1
	STF AC3,AC4	;AC4 ← X1
	STF AC0,AC5	;AC5 ← Y1
	MULF AC2,AC3	;AC3 ← X1Y2
	MULF AC1,AC0	;AC0 ← X2Y1
	SUBF AC0,AC3	;AC3 ← X1Y2 - X2Y1
	STF AC3,10(R0)	;Z ← AC3
	LDF 10(R2),AC0	;AC0 ← Z2
	LDF 10(R1),AC3	;AC3 ← Z1
	MULF AC4,AC0	;AC0 ← X1Z2
	MULF AC3,AC1	;AC1 ← X2Z1
	SUBF AC0,AC1	;AC1 ← X2Z1 - X1Z2
	STF AC1,4(R0)	;Y ← AC1
	LDF 10(R2),AC0	;AC0 ← Z2
	MULF AC5,AC0	;AC0 ← Y1Z2
	MULF AC2,AC3	;AC3 ← Y2Z1
	SUBF AC3,AC0	;AC0 ← Y1Z2 - Y2Z1
	STF AC0,(R0)	;X ← AC0
	MOV (R3)+,2(R3) ;Put result cell where first arg was 
	TST (R3)+	; & fix-up stack
	JSR PC,YESCMP	;OK to compact now
	CCC		;Clear condition code
	RTS PC		;Done

;TRANS extraction routines: TPOS, TORIEN, TAXIS, TMAGN

TPOS:	;Extracts the position part of a TRANS (last column)
	JSR 	PC,NOCMP	;Don't compact for a bit
	JSR	PC,GETVEC	;R0 ← -(R3) ← LOC[New Vector]
	MOV	2(R3),R1	;R1 ← LOC[TRANS]
	ADD	#44,R1		;R1 ← LOC [last column of TRANS]
	MOV	#6,R2		;Three 2-word components to move
1$:	MOV	(R1)+,(R0)+	;Copy it
	SOB	R2,1$
	MOV	ONE,(R0)+	;Stick in the scale factor
	CLR	(R0)
	MOV	(R3)+,(R3)	;Fix-up stack
	JSR 	PC,YESCMP	;OK to compact now
	CCC			;Clear condition codes
	RTS	PC		; & Return

TORIEN:	;Extracts the rotation part of a TRANS
	JSR 	PC,NOCMP	;Don't compact for a bit
	JSR	PC,GETTRN	;R0 ← -(R3) ← LOC[New TRANS]
	MOV	2(R3),R1	;R1 ← LOC[TRANS]
	MOV	#22,R2		;Three columns to do, three 2-word #'s/col
1$:	MOV	(R1)+,(R0)+	;Copy the ROTN
	SOB	R2,1$
	MOV	#6,R2
2$:	CLR	(R0)+		;Set up last column, three 0's
	SOB	R2,2$
	MOV	(R3)+,(R3)	;Fix-up stack
	JSR 	PC,YESCMP	;OK to compact now
	CCC			;Clear cond codes
	RTS	PC		; & Return

;TAXIS & TANGLE  routines to extract the axis vector and angle of rotation
;		 given a rotation (trans);

;Define some constants

DATA
ONE:	.FLT2	1.0
TWO:	.FLT2	2.0
CTHIRD:	.FLT2	0.576		;Square root of 1/3
C1001:	.FLT2	1.0001
C0001:	.FLT2	0.0001
CODE

TAXIS:	JSR	PC,TAXAN	;Get vector components in AC3,AC4 & AC5
	TST	(R3)+		;Fix stack
	JSR	PC,GETVEC	;Get a new vector to store results
	STF	AC3,(R0)+
	LDF	AC4,AC0
	STF	AC0,(R0)+	;Store X,Y & Z components
	LDF	AC5,AC0
	STF	AC0,(R0)+
	MOV	ONE,(R0)+	;Store scale factor of 1
	CLR	(R0)
	JSR 	PC,YESCMP	;OK to compact now
	CCC			;Clear condition codes
	RTS	PC		; & Return

TMAGN:	JSR	PC,TAXAN	;Get COS(angle) in AC0, vector components in AC 3-5
	STF	AC3,-(SP)	;Store X component
	JSR	PC,@LACOS	;Compute angle in AC0
	LDF	(SP)+,AC3	;Retrieve X
	LDF	CTHIRD,AC1	;Square root of 1/3
	LDF	AC3,AC2		;Get X
	ABSF	AC2
	CMPF	AC2,AC1		;ABS(X)-SQRT(1/3)
	CFCC			;Copy FPP cond codes into CPU cond codes
	BLT	1$
	LDF	34(R2),AC1	;Get (2,3)
	SUBF	24(R2),AC1	;(2,3) - (3,2)
	MULF	AC3,AC1		;Get sign of SIN(angle)
	BR	4$
1$:	LDF	AC4,AC2		;Get Y
	ABSF	AC2
	CMPF	AC2,AC1		;ABS(Y)-SQRT(1/3)
	CFCC			;Copy FPP cond codes into CPU cond codes
	BLT	2$
	LDF	10(R2),AC1	;Get (3,1)
	SUBF	30(R2),AC1	;(3,1) - (1,3)
	MULF	AC4,AC1		;Get sign of SIN(angle)
	BR	4$
2$:	LDF	AC5,AC2		;Get Z
	ABSF	AC2
	CMPF	AC2,AC1		;ABS(Z)-SQRT(1/3)
	CFCC			;Copy FPP cond codes into CPU cond codes
	BLT	3$
	LDF	14(R2),AC1	;Get (1,2)
	SUBF	4(R2),AC1	;(1,2) - (2,1)
	MULF	AC5,AC1		;Get sign of SIN(angle)
	BR	4$
3$:	ALERR	TMAGMS		;Complain
	CLRF	AC0		;& return NILROT
4$:	CFCC
	BLT	5$
	NEGF	AC0		;If SIN(angle) > 0 then negate angle
5$:	TST	(R3)+		;Clean up stack
	JSR 	PC,YESCMP	;OK to compact now
	JSR	PC,GETSCA	;Get a scalar
	STF	AC0,@(R3)	;Store the angle of rotation
	CCC			;Clear condition codes
	RTS	PC		; & Return

DATA
TMAGMS:	ASCIE	</ROTATION STRANGENESS/>
CODE

TAXAN:	;Code common to both TAXIS & TMAGN
	JSR 	PC,NOCMP	;Don't compact for a bit
	MOV	(R3),R2		;R2 points to the ROT
	LDF	(R2),AC0	;(1,1)
	ADDF	20(R2),AC0	;(2,2)
	ADDF	40(R2),AC0	;AC0 ← [(1,1)+(2,2)+(3,3)-1]/2 = COS(angle)
	SUBF	ONE,AC0
	STF	AC0,AC3		;we'll use this later
	DIVF	TWO,AC0
	STF	AC0,AC1		;Make a copy
	ABSF	AC1
	CMPF	C1001,AC1	;If ABS(COS(angle)) > 1.0001 return the NILROT
	CFCC
	BGT	1$		;Else go and compute the axis of rotation
	CLRF	AC0
	STF	AC0,AC3
	STF	AC0,AC4		;NILROT = 0 degrees about (0,0,1)
	LDF	ONE,AC1
	STF	AC1,AC5
	RTS	PC
1$:	STF	AC0,-(SP)	;Store COS(angle) away for later
	NEGF	AC3
	ADDF	TWO,AC3		;AC3 ← 3 - (1,1) - (2,2) - (3,3)
	LDF	ONE,AC0
	SUBF	(R2),AC0	;(1,1)
	SUBF	20(R2),AC0	;(2,2)
	ADDF	40(R2),AC0	;(3,3)
	DIVF	AC3,AC0		;AC0 ← Z↑2
	CMPF	C0001,AC0
	CFCC
	BLT	3$		;If Z > 0.0001 skip ahead
	CLRF	AC5		;Set Z ← 0
	LDF	ONE,AC0
	SUBF	(R2),AC0	;(1,1)
	ADDF	20(R2),AC0	;(2,2)
	SUBF	40(R2),AC0	;(3,3)
	DIVF	AC3,AC0		;AC0 ← Y↑2
	CMPF	C0001,AC0
	CFCC
	BLT	2$		;If Y > 0.0001 skip ahead
	CLRF	AC4		;Set Y ← 0
	LDF	ONE,AC3		;Set X ← 1
	BR	5$		;Skip to end
2$:	JSR	PC,@LSQRTF	;Get SQRT(Y↑2)
	STF	AC0,AC4
	LDF	AC5,AC2		;Clear this for later
	BR	4$		;Skip ahead to where X is computed
3$:	JSR	PC,@LSQRTF	;Get SQRT(Z↑2)
	STF	AC0,AC5
	LDF	ONE,AC2
	STF	AC2,AC3		;For later
	SUBF	(R2),AC2	;(1,1)
	LDF	14(R2),AC0	;(1,2)
	DIVF	AC2,AC0		;AC0 ← (1,2) / [ 1 - (1,1) ]
	LDF	10(R2),AC2	;(3,1)
	MULF	AC0,AC2
	ADDF	20(R2),AC2	;(3,2)
	MULF	4(R2),AC0	;(2,1)
	SUBF	AC0,AC3
	SUBF	20(R2),AC3	;(2,2)
	DIVF	AC3,AC2		;AC2 ← [(3,2)+(3,1)*(1,2)/[1-(1,1)] /
				;	[1-(2,2)-(2,1)*(1,2)/[1-(1,1)]
	MULF	AC5,AC2
	STF	AC2,AC4		;AC4 ← Y
	LDF	10(R2),AC2	;(3,1)
	MULF	AC5,AC2		;Z
4$:	LDF	4(R2),AC3	;(2,1)
	MULF	AC4,AC3		;Y
	ADDF	AC2,AC3
	LDF	ONE,AC1
	SUBF	(R2),AC1	;(1,1)
	DIVF	AC1,AC3		;AC3 ← [(2,1)*Y+(3,1)*Z] / [1-(1,1)] = X
5$:	LDF	(SP)+,AC0	;Retrieve the COS(angle)
	RTS	PC		; & Return to TAXIS or TMAGN
;Return vectors: SVMUL, TVMUL, VMAKE, VADD, VSUB

;83 -- 91 microseconds
SVMUL:	;Vector ← Scalar * Vector.  Interpreter routine
	;X ← S*X,  Y ← S*Y,  Z ← S*Z,  W ← W
	JSR PC,NOCMP	;Don't compact for a bit
	JSR PC,GETVEC	;R0 ← -(R3) ← LOC[new vector block]
	MOV 2(R3),R2	;R2 ← LOC[arg2]  (the vector)
	LDF @4(R3),AC0	;AC0 ← arg1 (the scalar)
	MOV #3,R1	;R1 ← 3:  How many fields to handle
1$:	LDF (R2)+,AC1	;AC1 ← next field of vector
	MULF AC0,AC1	;AC1 ← product
	STF AC1,(R0)+	;Store result
	SOB R1,1$	;Loop until all 3 fields done.
	MOV (R2)+,(R0)+	;Transfer W
	MOV (R2)+,(R0)+	;  which is 2 words long.
	MOV (R3)+,2(R3)	;Fix-up stack
	TST (R3)+
	JSR PC,YESCMP	;OK to compact now
	CCC		;Clear condition code
	RTS PC		;Done

VMAKE:	;Interpreter routine
	LDF @(R3)+,AC3	;Fetch arg3 (Z)
	LDF @(R3)+,AC2	;Fetch arg2 (Y)
	LDF @(R3)+,AC1	;Fetch arg1 (X)
	JSR PC,NOCMP	;Don't compact for a bit
	JSR PC,GETVEC	;R0 ← -(R3) ← LOC[new vector]
	STF AC1,(R0)+	;Store X
	STF AC2,(R0)+	;Store Y
	STF AC3,(R0)+	;Store Z
	MOV ONE,(R0)+	;Store W
	CLR (R0)	;Store W (second word)
	JSR PC,YESCMP	;OK to compact now
	CCC		;Clear condition code
	RTS PC		;Done

VADD:	;Interpreter routine
	JSR PC,NOCMP	;Don't compact for a bit
	MOV (R3)+,R0	;R0 ← LOC[arg 2] (a vector)
	MOV (R3)+,R1	;R1 ← LOC[arg 1] (a vector)
	LDF (R0)+,AC1	;Calculate X
	ADDF (R1)+,AC1
	LDF (R0)+,AC2	;Calculate Y
	ADDF (R1)+,AC2
	LDF (R0)+,AC3	;Calculate Z
	ADDF (R1)+,AC3
VRET:	JSR PC,GETVEC	;R0 ← -(R3) ← LOC[new vector]
	STF AC1,(R0)+	;Store X
	STF AC2,(R0)+	;Store Y
	STF AC3,(R0)+	;Store Z
	MOV ONE,(R0)+	;Assume W is 1
	CLR (R0)
	JSR PC,YESCMP	;OK to compact now
	CCC		;Clear condition code
	RTS PC		;Done

VSUB:	;Interpreter routine
	JSR PC,NOCMP	;Don't compact for a bit
	MOV (R3)+,R1	;R1 ← LOC[arg 2] (a vector)
	MOV (R3)+,R0	;R0 ← LOC[arg 1] (a vector)
	LDF (R0)+,AC1	;Calculate X
	SUBF (R1)+,AC1
	LDF (R0)+,AC2	;Calculate Y
	SUBF (R1)+,AC2
	LDF (R0)+,AC3	;Calculate Z
	SUBF (R1)+,AC3
	BR VRET		;Use common end code in VADD above

;283 -- 324 microseconds
TVMUL:	;Vector ← Trans * Vector.  Interpreter routine
	JSR PC,NOCMP	;Don't compact for a bit
	MOV (R3),R2	;R2 ← LOC[arg2] (the vector)
	MOV 2(R3),R0	;R0 ← LOC[arg1] (the trans)
	CLRF AC1	;X ← 0
	CLRF AC2	;Y ← 0
	CLRF AC3	;Z ← 0
	MOV #4,R1	;R1 ← How many columns left to go
1$:	LDF (R2)+,AC0	;AC0 ← field of vector
	STF AC0,AC5	;AC5 ← copy of AC0
	MULF (R0)+,AC0
	ADDF AC0,AC1	;Add partial result to X
	LDF AC5,AC0	;Restore AC0
	MULF (R0)+,AC0
	ADDF AC0,AC2	;Add partial result to Y
	LDF AC5,AC0	;Restore AC0
	MULF (R0)+,AC0
	ADDF AC0,AC3	;Add partial result to Z.
	SOB R1,1$	;Go back to do all 4 columns.
	JSR PC,GETVEC	;R0 ← -(R3) ← LOC[new vector]
	STF AC1,(R0)+	;Store X
	STF AC2,(R0)+	;Store Y
	STF AC3,(R0)+	;Store Z
	MOV -4(R2),(R0)+;Copy W from the vector
	MOV -2(R2),(R0)	;  (2 words long)
	MOV (R3)+,2(R3)	;Put result cell where first arg was
	TST (R3)+	; & fix up stack
	JSR PC,YESCMP	;OK to compact now
	CCC		;Clear condition code
	RTS PC		;Done
;Return a trans: TMAKE, TVADD, TVSUB, TTMUL, TINVRT, VSAXWR

TMAKE:	;Interpreter routine.
;All that is required is to take the rot part of the first argument,
;and the vector from the second part;
	JSR PC,NOCMP	;Don't compact for a bit
	JSR PC,GETTRN	;R0 ← -(R3) ← LOC[new trans]
	MOV 4(R3),R2	;R2 ← LOC[arg 1] (the trans)
	MOV #11,R1	;R1 ← Count of how many copies to make
1$:	MOV (R2)+,(R0)+	;Transfer first half of floating word
	MOV (R2)+,(R0)+	;Transfer second half of floating word
	SOB R1,1$	;Repeat until done
	MOV 2(R3),R2	;R2 ← LOC[arg 2] (the vector)
	MOV #3,R1	;R1 ← Count of how many copies to make
2$:	MOV (R2)+,(R0)+	;Transfer first half of floating word
	MOV (R2)+,(R0)+ ;Transfer second half of floating word
	SOB R1,2$	;Repeat until done
	MOV (R3)+,2(R3)	;Fix-up stack
	TST (R3)+
	JSR PC,YESCMP	;OK to compact now
	CCC		;Clear condition code.
	RTS PC		;Done.

TVCOM:	;Utility routine used to do common code in TVADD & TVSUB
	JSR PC,NOCMP	;Don't compact for a bit
	JSR PC,GETTRN	;R0 ← -(R3) ← LOC[new trans]
	MOV 2(R3),R2	;R2 ← LOC[arg 2] (the vector)
	MOV 4(R3),R1	;R1 ← LOC[arg 1] (the trans)
	MOV #11,R3	;R3 ← Count of how many copies to make
1$:	MOV (R1)+,(R0)+	;Transfer first half of floating word
	MOV (R1)+,(R0)+	;Transfer second half of floating word
	SOB R3,1$	;Repeat until done
	MOV #3,R3	;R3 ← Count of how many additions to perform
	RTS PC		;Return to TVADD or TVSUB

TVADD:	;Interpreter routine.
;All that is required is to take the rot part of the first argument,
;and add the vector from the first part to the second argument.
	PUSH <R3>	;Save R3
	JSR PC,TVCOM	;Do the common code for TVADD & TVSUB
1$:	LDF (R1)+,AC0	;AC0 ← word from trans
	ADDF (R2)+,AC0	;  + word from vector
	STF AC0,(R0)+	;
	SOB R3,1$	;Repeat until done
TVRET:	POP <R3>	;Restore R3
	MOV -2(R3),2(R3)	;Fix-up stack (pretty strange huh?)
	TST (R3)+
	JSR PC,YESCMP	;OK to compact now
	CCC		;Clear condition code.
	RTS PC		;Done.

TVSUB:	;Interpreter routine.
;All that is required is to take the rot part of the first argument,
;and subtract the second argument from the vector of the first arg.
	PUSH <R3>	;Save R3
	JSR PC,TVCOM	;Do the common code for TVADD & TVSUB
1$:	LDF (R1)+,AC0	;AC0 ← word from trans
	SUBF (R2)+,AC0	;  + word from vector
	STF AC0,(R0)+	;
	SOB R3,1$	;Repeat until done
	BR  TVRET	;Do common end code & return

TTMUL:	;Interpreter routine
;Multiplies two transes together.
	PUSH <R4>	;Save R4
	JSR PC,NOCMP	;Don't compact for a bit
	JSR PC,GETTRN	;R0 ← -(R3) ← LOC[new trans]
	MOV 2(R3),R2	;R2 ← LOC[arg 2]
	MOV 4(R3),R4	;R4 ← LOC[arg 1]
	PUSH <R3,R4>	;Save R3 & a copy of R4
	MOV #4,R1	;Loop count for cols of answer
1$:	LDF (R2)+,AC1	;Pick up a column of arg2: First row
	LDF (R2)+,AC2	;  Second row
	LDF (R2)+,AC3	;  Third row
	STF AC3,AC4	;    store in AC4
	MOV #3,R3	;Loop count for rows of answer
2$:	LDF (R4),AC3	;First col of arg 1
	MULF AC1,AC3
	LDF 14(R4),AC0	;Second col of arg 1
	MULF AC2,AC0
	ADDF AC0,AC3
	LDF 30(R4),AC0	;Third col of arg 1
	MULF AC4,AC0	;
	ADDF AC0,AC3	;
	STF AC3,(R0)+	;
	ADD #4,R4	;Move to the next column of arg 1
	SOB R3,2$	;Repeat for first 3 rows of answer
	MOV (SP),R4	;Reset R4 to point to first row of arg 1
	SOB R1,1$	;Repeat for all four columns of answer
	LDF -14(R0),AC1	;Add correction for last column, first row
	ADDF 44(R4),AC1
	STF AC1,-14(R0)
	LDF -10(R0),AC1	;Add correction for last column, second row
	ADDF 50(R4),AC1
	STF AC1,-10(R0)
	LDF -4(R0),AC1	;Add correction for last column, third row
	ADDF 54(R4),AC1
	STF AC1,-4(R0)
	TST (SP)+	;Pop the R4 temp
	POP <R3,R4>	;Restore R3 & R4
	MOV (R3)+,2(R3)	;Fix-up stack
	TST (R3)+
	JSR PC,YESCMP	;OK to compact now
	CCC		;Clear condition code
	RTS PC		;Done

TINVRT:	;Interpreter routine
COMMENT ⊗ Inverts a trans.
	  The result, (rot',trslat'), is defined:
					rot' = transpose(rot)
					trslat' = -(rot'*trslat)
⊗
	JSR PC,NOCMP	;Don't compact for a bit
	JSR PC,GETTRN	;R0 ← -(R3) ← LOC[new trans] + 4*interation number
	MOV 2(R3),R2	;R2 ← LOC[old trans], travels down the whole trans
	PUSH <R3,R4>	;Save R3 & R4
	MOV R0,R3	;R3 ← LOC[new trans] + 20*interation number
	MOV R2,R4	;R4 ← LOC[old trans], stays constant
	MOV #3,R1	;Three columns to do
1$:	;Transpose a column, multiplying by the translation
	CLRF AC1	;Cumulative product
	LDF (R2)+,AC0	;Take from the source rotation
	STF AC0,(R0)	;  into the transpose,
	MULF 44(R4),AC0
	SUBF AC0,AC1	;accumulate the product.
	LDF (R2)+,AC0	;Take from the source rotation
	STF AC0,14(R0)	;  into the transpose,
	MULF 50(R4),AC0
	SUBF AC0,AC1	;accumulate the product.
	LDF (R2)+,AC0	;Take from the source rotation
	STF AC0,30(R0)	;  into the transpose
	MULF 54(R4),AC0
	SUBF AC0,AC1	;accumulate the product
	STF AC1,44(R0)	;Place the new translation
	ADD #4,R0	;Move to next row of result
	ADD #14,R3	;Move to next column of result
	SOB R1,1$
	POP <R4,R3>	;Restore R4 & R3
	MOV (R3)+,(R3)	;Fix-up stack
	JSR PC,YESCMP	;OK to compact now
	CCC		;Clear condition code
	RTS PC		;Done

VSAXWR:	;Interpreter Routine	coded by ARG 5/3/76
;Produces a trans that rotates about a vector by a given angle
	PUSH <R5>		;Save R5
	LDF @(R3)+,AC2		;Save angle in AC2
	JSR PC,UNITV		;Convert vector to unit vector
	STF AC2,AC0		;Retrieve angle
	JSR PC,@LSNCSD		;Get sin & cos of angle
	STF AC0,AC4		;Save sin in AC4
	STF AC1,AC5		;Save cos in AC5
	SUBF ONE,AC1		;AC1←(1-COS)
	NEGF AC1
	JSR PC,NOCMP		;Don't compact for a bit
	JSR PC,GETTRN		;R0←-(R3)←LOC[New Tran]
	MOV 2(R3),R1		;R1←LOC[Unit Vec]
	PUSH <#3>		;Three columns to do
1$:	MOV #3,R5		;Three rows to do
	MOV 2(R3),R2		;R2←LOC[Unit vec]
	LDF AC1,AC2
	MULF (R1)+,AC2		;AC2←(1-COS)*U[i]
2$:	LDF AC2,AC3
	MULF (R2)+,AC3		;Trans[j,i]←(1-COS)*U[i]*U[j]
	STF AC3,(R0)+
	SOB R5,2$		;Do all 3 rows
	DEC (SP)
	BGT 1$			;Do all 3 columns
	POP <(R0)+>
	CLR (R0)+		;Set up last column
	CLR (R0)+
	CLR (R0)+
	CLR (R0)+
	CLR (R0)+
	MOV #3,R5		;Three terms to do: (1,1) (2,2) & (3,3)
	MOV (R3),R0		;R0←LOC[Trans]
3$:	LDF AC5,AC1		;AC1←COS
	ADDF (R0),AC1		;Add COS to (1-COS)*U[i]*U[i] term
	STF AC1,(R0)
	ADD #20,R0		;R0 points to next term to add COS to
	SOB R5,3$		;Do all three terms
	MOV (R3),R0		;R0←LOC[Trans]
	MOV 2(R3),R1		;R1←LOC[Unit Vec]
	LDF (R1)+,AC2		;AC2←U[X]
	MULF AC4,AC2		;AC2←SIN*U[X]
	STF AC2,AC3		;Make a copy
	ADDF 24(R0),AC2		;Add it to the (3,2) term
	STF AC2,24(R0)
	NEGF AC3
	ADDF 34(R0),AC3		;Subtract it from the (2,3) term
	STF AC3,34(R0)
	LDF (R1)+,AC2		;AC2←U[Y]
	MULF AC4,AC2		;AC2←SIN*U[Y]
	STF AC2,AC3		;Make a copy
	ADDF 30(R0),AC2		;Add it to the (1,3) term
	STF AC2,30(R0)
	NEGF AC3
	ADDF 10(R0),AC3		;Subtract it from the (3,1) term
	STF AC3,10(R0)
	LDF (R1)+,AC2		;AC2←U[Z]
	MULF AC4,AC2		;AC2←SIN*U[Z]
	STF AC2,AC3		;Make a copy
	ADDF 4(R0),AC2		;Add it to the (2,1) term
	STF AC2,4(R0)
	NEGF AC3
	ADDF 14(R0),AC3		;Subtract it from the (1,2) term
	STF AC3,14(R0)		;Trans is done!
	POP <R5>		;Restore R5
	MOV (R3)+,(R3)		;Clean up stack
	JSR PC,YESCMP		;OK to compact now
	CCC			;Clear condition codes
	RTS PC			; & Return

;Motion:  MOVE, CENTER

MOVE:	;Interpreter routine
	MOV LMOVE,R2	;Set for moving operation
	JMP MOVSTA	;Use the common move code

CENTER:	;Interpreter routine
	MOV LCENTER,R2	;Set for centering operation
	JMP MOVSTA	;Use the common move code

DVBKSZ == 12		;Size of a device block

COMMENT ⊗ New version to update the frame afterwords.  Assumes that
there are two arguments: a pointer to the trajectory table and a word
of mechanism bits, to help in updating the necessary variables.  ⊗

MOVSTA:	MOV #'π,R0	;Whistle while you work
	JSR PC,TYPCHR
	MOV #DVBKSZ,R0	;Get a device block
	JSR PC,GTFREE
	MOV R0,R1	;R1 ← address of device block
	PUSH <R0>	;Save a copy on the stack
	MOV @IPC(R4),R0	;R0 ← address of coefficient list
	JSR PC,NOCMP	;Don't compact for a bit
	JSR PC,@R2	;Do some kind of move (MOVE, CENTER)
	JSR PC,YESCMP	;OK to compact now
	TST R0		;Did the move succeed?
	BEQ 1$		;Yes
	PUSH <R0>	;  save error code
	MOV #MVMES1,R0
	JSR PC,TYPSTR	;  Complain
	POP <R0>
	BIT #177400,R0	;Associated joint #?
	BEQ 12$		; No skip ahead
	MOV (SP),R1	;Get address of device block
	MOV (R1)+,R2	;Maximum number of joints in device block
	TST (R1)+	;Point to first joint
10$:	BIT #177400,(R1)+	;Is this the offending joint?
	BNE 11$		; Yup - found it
	SOB R2,10$	;Try next joint
	BR 12$
11$:	MOV -(R1),R0	;Change R0 so the low 2 digits give joint #
12$:	JSR PC,TYPOCT	;  Give error condition
	MOV (SP),R1	;  put address of device block in R1
	ALERR MOVERR	;  and switch to DDT
1$:	BMPIPC		;Bump IPC
	FETCH R2	;R2 ← mechanism bits
	;Invalidate the affected device variables;
    .IFNZ YELLOW
	BIT #YARM,R2
	BEQ 2$
	MOV #YARMHD,R0	;Header for YARM
	JSR PC,INVLDT
2$:	BIT #YHAND,R2
	BEQ 3$
	MOV #YHANDH,R0	;Header for YHAND
	JSR PC,INVLDT
    .ENDC
3$:	BIT #BARM,R2
	BEQ 4$
	MOV #BARMHD,R0	;Header for BARM
	JSR PC,INVLDT
4$:	BIT #BHAND,R2
	BEQ 5$
	MOV #BHANDH,R0	;Header for BHAND
	JSR PC,INVLDT
5$:	POP <R0>
	JSR PC,RLFREE	;Get rid of the device block
	CCC		;Clear condition code
	RTS PC		;Return
RETRY:	TST (SP)+	;Get here from ALERR; clean off stack
	POP <R0>
	JSR PC,RLFREE	;Get rid of the device block
	BACKIPC		;Backup IPC
	RTS PC

DATA
MVMES1:	ASCIE </
SERVO ERROR = />
MOVERR:	ASCIE </DEVICE BLOCK AT (R1). TO RETRY THE MOVE, RETRY$G/>
CODE

;	STOP, TABOFS, WHERE, NOTICE

STOP:	;Interpreter routine
COMMENT ⊗ Takes one argument, a set of mechanism bits.  (e.g. BARM,
BHAND, YARM, YHAND).  For each one on, all the associated joints are
stopped.  ⊗

	FETCH R2	;R2 ← mechanism bits
	MOV R2,R0	;R0 ← mech bits
	JSR PC,TABOFS	;R0 ← table offset
	BIT #AHAND,R2	;A hand?
	BNE 1$		;Yes
	MOV #6,R1	;R1 ← count of joints
	BR 2$
1$:	MOV #1,R1	;R1 ← count of joints
2$:	ADD LDVCPTR,R0	;R0 ← LOC[table of device pointers]
3$:	MOV (R0)+,R2	;R2 ← device block
	BEQ 4$		;If any
	BIS #100000,@0(R2)	;Stop this device.
4$:	SOB R1,3$	;Repeat
	CCC		;Clear condition code
	RTS PC		;Done

COMMENT ⊗ Certain tables are available via COMTAB entries.  LERRPTR
points to the table ERRPTR of 16 words, one for each servo, which
points at the current error torques.  LTHPTR points at the table THPTR
of 16 words, one for each servo, which points at the current joint
angles.  ⊗

TABOFS:	
COMMENT ⊗ R0 = Mechanism bit.  Returns table offset (in bytes) in R0.
For example, if the mechanism is BARM, the OFBARM is returned.  ⊗
	BIT #YARM,R0	;Is it this mechanism?
	BEQ 1$		;No
	MOV #OFYARM,R0	;Yes.  Load up proper offset
	RTS PC		; and return.
1$:	BIT #YHAND,R0	;Is it this mechanism?
	BEQ 2$		;No
	MOV #OFYHAND,R0	;Yes.  Load up proper offset
	RTS PC		; and return.
2$:	BIT #BARM,R0	;Is it this mechanism?
	BEQ 3$		;No
	MOV #OFBARM,R0	;Yes.  Load up proper offset
	RTS PC		; and return.
3$:	BIT #BHAND,R0	;Is it this mechanism?
	BEQ 4$		;No
	MOV #OFBHAND,R0	;Yes.  Load up proper offset
	RTS PC		; and return.
4$:	ALERR TABMES	;Illegal
	CLR R0
	RTS PC
DATA
TABMES:	ASCIE </ILLEGAL MECHANISM/>
CODE

WHERE:	;Interpreter routine
COMMENT ⊗ One argument: The mechanism bits.  Puts value of that
mechanism on the stack.  Only one mechanism at a time, please!  ⊗
	FETCH R2	;Mechanism bits
	JSR PC,NOCMP	;Don't compact for a bit
	BIT #AHAND,R2	;A hand?
	BNE 1$		;No.
	JSR PC,GETTRN	;R0 ← -(R3) ← LOC[new trans]
	BR 2$
1$:	JSR PC,GETSCA	;R0 ← -(R3) ← LOC[new scalar]
2$:	MOV LTHPTR,R1	;
	JSR PC,@LUPDATE	;
	JSR PC,YESCMP	;OK to compact now
	CCC		;Clear condition code
	RTS PC		;Done

NOTICE:
COMMENT ⊗ The arms may have been moved without our knowledge.  A call
to this routine calls @LWHERE to find the real locations, and
invalidates all manipulator variables.  This routine should be called
from WAITE, but not from MOVE or CENTER.  It may be called from DDT,
since it saves all registers.  ⊗
	PUSH <R0,R1>	;Save R0 & R1
	STF AC0,-(SP)	;Save AC0
	MOV #DVBKSZ,R0	;Get a device block
	JSR PC,GTFREE	;
	MOV R0,R1	;R1 ← address of device block
	PUSH <R0>	;Save a copy on the stack
	MOV #MVDCOF,R0	;Pointer to coefficient list
	JSR PC,@LWHERE	;Update the servo blocks
			;ignore any failure return.
	POP <R0>	;Reclaim device block
	JSR PC,RLFREE	;
	;Invalidate all manipulator variables
	MOV #BHANDH,R0
	JSR PC,INVLDT
	MOV #BARMHD,R0
	JSR PC,INVLDT
    .IFNZ YELLOW
	MOV #YHANDH,R0
	JSR PC,INVLDT
	MOV #YARMHD,R0
	JSR PC,INVLDT
    .ENDC
	LDF (SP)+,AC0	;Restore AC0
	POP <R1,R0>	;Restore R1 & R0
	RTS PC

DATA
    .IFNZ YELLOW
MVDCOF:	YHANDSB + YARMSB + BHANDSB + BARMSB
	0
    .IFF
MVDCOF:	BHANDSB + BARMSB
	0
    .ENDC
CODE
;Condition monitors:  CMMAK 

COMMENT ⊗ This is the third version of condition monitors: modified by arg 5/77
(here refered to as c-m's).  Hardware-type c-m's will be ready soon.
The basic operations are Creation, Enabling, Disabling, Destruction.
Creation causes a c-m control block to be set up, and pointed to by
the c-m variable.  This block has the following fields: ⊗

	II == 0
	XX	CMTYPE	;Type of c-m: event,expression,duration,force or hardware
	    CMEVT == 0		;Event type c-m
	    CMEXP == 1		;Expression type c-m
	    CMDRA == 2		;Duration type c-m
	    CMFRC == 3		;Force sensing type c-m
	    CMHRD == 4		;Hardware monitor type c-m
	XX	CMSTRT	;Starting address of c-m: duration, force & hardware
	XX	CMISB	;LOC[ISB] of the c-m
	XX	CMBITS	;Bits needed for: force & hardware c-m's
	XX	CMSTAT	;Status bits for the c-m
            CMENB == 1               ;set => enabled
            CMDES == 2               ;set => to be destroyed
            CMRUN == 4               ;set => c-m is currently running
	CMCBSZ == II/2	;Length in words of a c-m control block.
	II == 4		;for event & expression c-m's
	XX	CMSEVT	;The event used to awaken the tester upon enabling
	XX	CMTEVT	;The event for which this c-m tests, if any

COMMENT ⊗ The various types of condition monitors are each handled
differently. Basically each c-m is an independent process which runs
in parallel with the process that creates it. Each c-m is an interpreter
and runs at priority 1 (exception: the checker part of an expression c-m
runs at priority 3). When a c-m is created by CMMAK, new PDB, ISB and
CMCB blocks are made. For duration, force and hardware c-m's nothing
further is done until they are enabled or destroyed. Enabling causes
the c-m checker part to be interpreted and to place the c-m body in the
appropriate queue, so it will be run if & when the condition being
checked for occurs. Disabling removes the c-m from the queue. Destroying
the c-m causes it to be disabled and then it's PDB, ISB & CMCB are all
reclaimed. At the conclusion of the body if the c-m has been re-enabled
it reschedules itself in the appropriate queue and then dismisses.
     Event and expression c-m's, after initialization, wait for the
gronking event CMSEVT.  Enabling signals the event CMSEVT and sets
the enabled bit in CMSTAT.  Disabling resets the enabled bit, and the
c-m will wait on the CMSEVT for future action.  As long as the c-m is
enabled, it periodically wakes up, checks its status bits.  If the
enable bit is reset, the c-m waits for CMSEVT.  Else it checks the
condition.  If it is satisfied, the c-m disables itself and
proceeds to the conclusion (at level 1, the conclusion should reset
itself to level 0 after all critical activity has been accomplished,
although this is not currently done.) Otherwise, it reschedules itself.
If the destroy bit should ever be set in CMSTAT, then the c-m will
destroy the event CMSEVT.  Then it will reclaim the c-m control blocked
and will dismiss, never to return. (The pointer to the c-m in the
environment should be zeroed by the destroying angel.). ⊗

CMMAK:	;Auxillary routine

COMMENT ⊗ Takes three arguments, the offset of the nascent c-m, the
level-offset of the event that this monitor is to wait on, if any,
and the IPC of the c-m code. Called with the number of cmons to make
in R0 and R2 pointing at the environment entry. ⊗

1$:	PUSH <R0>		;Save count of how many cmons to make

	;Make a c-m control block
	MOV #CMCBSZ,R0
	JSR PC,GTFREE		;R0 ← LOC[c-m control block]
	MOV #CMNTYP,(R2)+	;Set data type to cmon
	MOV R0,(R2)+		;Stuff into environment
	PUSH <R2>		;Save environment pointer
	FETCH CMTYPE(R0)	;Get type of c-m
	PUSH <R0>		;Save LOC[c-m control block]

	;Prepare the c-m job
	FETCH CMSTRT(R0)	;Store away IPC of start of c-m code
	MOV CMSTRT(R0),R0	;R0 ← IPC of c-m code
	CLR R1			;C-m's do not expire with events
	JSR PC,SPAWN		;R0 ← process control block for c-m
	MOV (SP),R1		;R1 ← LOC[CMCB]
        MOV PDBR4(R0),R2	;R2 ← PR4 = LOC[c-m's interpeter status block]
	MOV R2,CMISB(R1)	;Store away location of c-m's ISB
        MOV R1,CMCB(R2)		;Stuff CMCB of the c-m
	MOV #UGRSAV+UFPUSE+2,PDBSTA(R0)	;c-m's run with priority = 1
	MOV #144040,UPSW(R0)
	CMP #CMEXP,CMTYPE(R1)	;If expression c-m runs with priority = 3
	BNE 2$
	MOV #UGRSAV+UFPUSE+6,PDBSTA(R0)	;Change priority to 3
	MOV #144140,UPSW(R0)
2$:	MOV R0,R2		;R2 ← new process descriptor block 

	;Set up the new environment
	JSR PC,NEWENV	;R0 ← LOC[new environment]
	MOV ENV(R4),SLINK(R0)	;Not necessary to set up OLEV, etc.
	MOV PDBR4(R2),R1
	MOV R0,ENV(R1)
   .IFNZ SMALLB
	EVSIG SBEVT		;End of critical section - value stored
   .ENDC
	INC LEV(R1)

	POP <R0>		;R0 ← LOC[CMCB]
	CMP #CMEXP,CMTYPE(R0)	;See what type of c-m we've got
	BLT 4$			;Duration, force sensing or hardware - jump ahead
	BEQ 3$			;Expression c-m  - skip over event c-m stuff
	PUSH <R0>		;Save LOC[c-m control block]
	FETCH R0		;R0 ← level-offset of event this c-m waits for.
	JSR PC,GETARG
	MOV 2(R0),R1		;R1 ← identifier of event]
	POP <R0>		;R0 ← LOC[c-m control block]
	MOV R1,CMTEVT(R0)	;Put the CMTEVT in the c-m control block.
3$:	EVMAK
	POP <CMSEVT(R0)>	;Make an event for CMSEVT
	FORK R2,#INTERP,#USRDM	;Cause the c-m to be started.  It will go into wait.
	BR 5$			;Done

4$:	CMP #CMDRA,CMTYPE(R0)
	BEQ 5$			;If duration type then done
	FETCH CMBITS(R0)	;Get force sensing bits for c-m

5$:	POP <R2,R0>		;Retrieve env pointer & count
	SOB R0,1$		;  & make as many cmons as we were told to
	RTS PC			;Done

;  CMENBL, CMDSBL, CMDEST, CMTRIG, CMSKED, CMUNCR

CMENBL: ;Interpeter routine
;  One argument, a level-offset pair for the c-m to enable.
	FETCH R0	;R0 ← level-offset
	JSR PC,GETARG	;R0 ← pointer into environment
	MOV 2(R0),R0	;R0 ← pointer to c-m control block.
	BEQ CMDERR	;If none, then error
	CMP #CMDRA,CMTYPE(R0)	;see what type of c-m we've got
	BGT 1$		;If event or expression then skip ahead
	BIT #CMENB,CMSTAT(R0)	;Already enabled?
	BNE 2$			;Then done
	BIS #CMENB,CMSTAT(R0)	;Set enabled bit
	BIT #CMRUN,CMSTAT(R0)	;See if currently running
	BNE 2$			; & if so we're done - it'll re-enable itself
	PUSH <R4>		;Save old ISB
	MOV CMISB(R0),R4	;Get new ISB
	MOV CMSTRT(R0),IPC(R4)	;Set IPC to LOC[c-m checker]
	MOV RF,-(SP)		;Save RF
	MOV SP,RF		;RF ← LOC[Stack]
	JSR PC,INTERP		;Go do it - CMDUR, CMFORCE & CMSENSE return
	POP <R4>		;Restore old ISB
	BR 2$		;Done
1$:	BIS #CMENB,CMSTAT(R0)	;Set the enable bit
	EVSIG CMSEVT(R0)	;Gronk the c-m
2$:	CCC		;Clear condition code
	RTS PC		;Done

CMDSBL:	;Interpreter routine
;  One argument, a level-offset pair for the c-m to disable.
	FETCH R0	;R0 ← level-offset
	JSR PC,GETARG	;R0 ← pointer into environment
	MOV 2(R0),R0	;R0 ← pointer to c-m control block.
	BEQ CMDERR	;If none, then error
	JSR PC,CMDIS	;Go disable the c-m
	CCC		;Clear condition code
	RTS PC		;Done
CMDERR:	ALERR CMNEMS
	SCC		;Set condition code
	RTS PC
DATA
CMNEMS:	ASCIE </TRYING TO TREAT NON-EXISTENT EVENT/>
CODE

CMDEST:	;Auxillary routine
COMMENT ⊗ Called by KVAR to kill the cmon pointed at by (R2). ⊗
	PUSH <R1,R2>		;Save R1 & R2
	MOV (R2),R0		;R0 ← LOC[c-m control block]
	BEQ CMDERR		;If none, then error
	BIS #CMDES,CMSTAT(R0)	;Set the destroy bit
	CMP #CMDRA,CMTYPE(R0)	;See what type of c-m
	BGT 2$			;If event or expression c-m then handle below
	BIT #CMRUN,CMSTAT(R0)	;If running it will destroy itself
	BNE 3$			; so we're done
	JSR PC,CMDIS		;Make sure c-m's disabled
	MOV CMISB(R0),R2	;R2 ← LOC[c-m's ISB]
	JSR PC,RLFREE		;Reclaim the c-m control block
	MOV STKBAS(R2),R0	;Reclaim interpreter stack
	JSR PC,RLFREE
	MOV PDB(R2),R0		;Reclaim Process Descriptor Block
	JSR PC,RLFREE
	EVWAIT INTEVT		;Enter critical region.
	MOV #ISTBLK,R0	;The following unlinks this interpreter from the chain.
1$:	MOV R0,R1
	MOV NXTINT(R1),R0
	CMP R0,R2		;Have we found ours yet?
	BNE 1$
	MOV NXTINT(R2),NXTINT(R1)	; Yes. rechain.
	EVSIG INTEVT		;Leave critical region.
	MOV R2,R0		;Reclaim Interpreter Status Block
	JSR PC,RLFREE
	BR 3$
2$:	EVKIL CMSEVT(R0)	;Destroy the event.  That ought to wake him up!
3$:	POP <R2,R1>
	RTS PC			;Done

CMDIS:
COMMENT ⊗ Routine to disable a c-m, R0 ← LOC[CMCB] ⊗
	BIT #CMENB,CMSTAT(R0)	;Check if currently enabled
	BEQ 3$			; if not - done
	CMP #CMDRA,CMTYPE(R0)	;See what type of c-m
	BGT 2$			;Event & expression c-m's are easy - skip ahead
	BEQ 2$			;Can't do anything with duration c-m's now
	CMP #CMFRC,CMTYPE(R0)
	BLT 2$			; ditto with hardware c-m's
	PUSH <R0>		;Save R0
	MOV CMISB(R0),R1	;R1 ← LOC[c-m's ISB]
	MOV PDB(R1),R1		;R1 ← LOC[c-m's PDB]
	MOV CMBITS(R0),R0	;R0 ← c-m's force sensing bits
	JSR PC,@LFRCOFF		;Remove c-m from force signal list
	TST R0
	BEQ 1$
	ALERR CMNODS		;Complain if error
1$:	POP <R0>		;Restore R0
2$:	BIC #CMENB,CMSTAT(R0)	;Clear the enable bit
3$:	RTS PC			;Done

DATA
CMNODS:	ASCIE </COULDN'T DISABLE FORCE CMON/>
CODE

CMTRIG:	;Interpeter routine
COMMENT ⊗ Should be executed only from a c-m.  Sets the priority to 1
and disables the checker.  ⊗

	MOV CMCB(R4),R0
1$:	EVTST CMSEVT(R0)	;Eat all signals enabling the checker.
	BCC 1$
	BIC #CMENB,CMSTAT(R0)	;Clear the enable bit
	SETPRI #1		;Set the priority to 1
	TST (SP)+		;Discard old priority
	CCC			;Clear condition code
	RTS PC			;Done

CMSKED:	;Interpreter routine
COMMENT ⊗ Goes to sleep a while (currently, 100 milliseconds).  Upon
awakening, checks the status bits of this checker, and either
dismisses, waits, or returns.  ⊗

	MOV CMCB(R4),R0	;R0 ← c-m control block
	CMP #CMEXP,CMTYPE(R0)	;See what type of c-m
	BNE 1$		;If event c-m skip ahead
	SETPRI #3	;In case the conclusion left it at 1 or 0.
	TST (SP)+	;Flush old priority
	FETCH -(SP)	;Waiting interval
	SLEEP 		;Sleep a while
1$:	BIT #CMDES,CMSTAT(R0)	;Destroy bit set?
	BEQ 3$		;No
	EVKIL CMSEVT(R0);Yes.  Kill the triggering event.
2$:	JSR PC,RLFREE	;Return the c-m control block
	JMP TERMINATE	;Use the interpeter terminate routine.
3$:	BIT #CMENB,CMSTAT(R0)	;Enable bit set?
	BNE 4$		;Yes.
	EVWAIT CMSEVT(R0);No.  Wait until signaled by the enabler
	BCS 2$		;If the enabling event died, so must we.
	BR  1$		;Else start from the awakening point.
4$:	MOV CMTEVT(R0),R1	;R1 ← event to test for
	BEQ 5$		;If any
	EVWAIT R1	;Wait for event to happen
	BCS 2$		;If the signaling event died, so must we.
	BIT #CMENB,CMSTAT(R0)	;Still enabled?
	BNE 5$		;Yes.  May exit.
	EVSIG R1	;Oops, we were disabled!  Resignal the event.
	BR 1$		;And try again.
5$:	CCC		;Clear condition code
	RTS PC		;Done

CMUNCR:	;Interpreter routine.  
COMMENT ⊗  Used in body of c-m.  Starts uncritical section.  ⊗

	SETPRI #1	;Set the priority to 1
	TST (SP)+	;Flush old priority
	CCC		;Clear condition code
	RTS PC		;Done

;  CMDONE, CMDUR, CMFORCE, CMSENSE, COMPLY, CMPOFF, VMKFRC, TFRCST

CMDONE:	;Interpreter routine
COMMENT ⊗ Ends duration, force sensing & hardware monitor c-m's. Checks if
c-m was re-enabled while running and if so it will interpret the c-m's checker
(and so doing the c-m will be re-queued). Then it dismisses. ⊗
	MOV CMCB(R4),R0		;Get c-m control block
	BIC #CMRUN,CMSTAT(R0)	;Clear run bit
	BIT #CMDEST,CMSTAT(R0)	;Destroy ourself
	BEQ 1$
	JSR PC,RLFREE		;Yup - reclaim CMCB
	JMP TERMINATE		;Use interpreter terminate routine
1$:	BIT #CMENB,CMSTAT(R0)	;See if we were re-enabled
	BEQ 2$			;Nope - go away
	MOV CMSTRT(R0),IPC(R4)	;Reset IPC to LOC[c-m's checker]
	MOV RF,-(SP)		;Save RF
	MOV SP,RF		;RF ← LOC[Stack]
	JSR PC,INTERP		;Re-queue it
2$:	MOV PDB(R4),R0		;R0 ← LOC[c-m's PDB]
	MOV R3,PDBR3(R0)	;Make sure stack is okay
	MOV PDBPC(R0),PDBR2(R0)	;Save new PC(if any) in R2 since DISMIS kills it
	DISMIS			;Bye-bye
	JMP (R2)		;If return here use R2 to get where we should be

CMDUR:	;Interpreter routine
COMMENT ⊗ Schedules c-m body to be executed in time seconds. (The time is
on the stack.) Then returns control using RF. ⊗
	LDF @(R3)+,AC0		;Get time to wait in seconds
	MULF THOUS,AC0		;Convert it to milliseconds
	STCFI AC0,R0		; & make it integer
	SCHEDU PDB(R4),#1$,#USRDM,R0	;Schedule the c-m body to start later
	MOV RF,SP		;Restore stack
	MOV -2(SP),RF		;RF ← old PC
	RTS RF			;Just return

1$:	MOV CMCB(R4),R0		;R0 ← LOC[c-m's control block]
	BIT #CMENB,CMSTAT(R0)	;See if we're still enabled
	BNE 2$
	DISMIS			;If not then go away
2$:	JMP CMGO		;Set flags & go interpret the c-m's body

VMKFRC:	;Interpreter routine
COMMENT ⊗ Takes force vector (on R3 stack) and makes it into a frame with the x-axis
along the force vector. Always followed by a call to TFRCST which calls SETC. ⊗
	JSR PC,UNITV		;Make it a unit vector
	JSR PC,NOCMP		;Don't compact for a while
	MOV (R3)+,R0		;R0 ← LOC[unit vector]
	LDF (R0)+,AC0		;Get X
	LDF (R0)+,AC1		;Get Y
	LDF (R0)+,AC2		;Get Z
	STF AC0,AC4		;Copy X
	STF AC1,AC5		;Copy Y
	JSR PC,GETTRN		;R0 ← -(R3) ← LOC[new trans]
	STF AC0,(R0)+
	STF AC1,(R0)+		;Fill in 1st column with unit vector
	STF AC2,(R0)+
	MULF AC0,AC0		;X↑2
	MULF AC1,AC1		;Y↑2
	ADDF AC1,AC0		;X↑2 + Y↑2
	CFCC			;Check if X = Y = 0
	BNE 1$			; & if not skip ahead
	CMP (R0)+,(R0)+
	STF AC2,(R0)+		;Next column is (0 Z 0)
	NEGF AC2
	CMP (R0)+,(R0)+
	STF AC2,(R0)		;Last column is (-Z 0 0)
	BR 2$			;Jump ahead
1$:	JSR PC,@LSQRTF		;get SQRT(X↑2 + Y↑2)
	STF AC0,AC1		;Copy SQRT(X↑2 + Y↑2)
	NEGF AC5		;Negate Y
	DIVF AC5,AC0		;a = -Y / SQRT(X↑2 + Y↑2)
	DIVF AC4,AC1		;b =  X / SQRT(X↑2 + Y↑2)
	STF AC0,(R0)+
	STF AC1,(R0)+		;Fill in 2nd column with (a b 0)
	CMP (R0)+,(R0)+
	STF AC2,AC3		;Copy Z
	MULF AC0,AC2		;aZ
	MULF AC1,AC3		;bZ
	NEGF AC3		;-bZ
	MULF AC4,AC1		; bX
	MULF AC5,AC0		;-aY
	ADDF AC0,AC1		;bX - aY
	STF AC3,(R0)+
	STF AC2,(R0)+		;Fill in 3rd column with(-bZ,aZ,bX-aY)
	STF AC1,(R0)+		;  it's the cross product of the other 2 columns
2$:	JSR PC,YESCMP		;OK to compact again
	CCC
	RTS PC			;Done - return

TFRCST:	;Interpreter routine
COMMENT ⊗ Gets force frame off of the R3 stack, arm & co-ordinate system bits follow
via the IPC. Calls SETC. ⊗
	FETCH R0		;Get bits for SETC: arm & c-oord system (hand/table)
	MOV (R3)+,R1		;R1 ← LOC[force coordinate matrix]
	JSR PC,@LSETC		;Initialize the force system
	TST R0
	BEQ 1$
	ALERR CMNSET		;Complain if any problems
1$:	CCC
	RTS PC			;Done - return

CMFORCE: ;Interpreter routine
COMMENT ⊗ Gets force value (scalar on R3 stack) and queues c-m on force signal list.
Then returns control using RF. ⊗
	LDF @(R3)+,AC0		;Get the force threshold value
	MOV PDB(R4),R1		;R1 ← LOC[c-m's PDB]
	MOV CMCB(R4),R2		;R2 ← LOC[c-m's control block]
	MOV CMBITS(R2),R0	;R0 ← c-m's force bits
	MOV #CMGO,R2		;R2 ← when triggered start below
	JSR PC,@LFRCSIG		;Put the c-m in the force signal list
	TST R0
	BEQ 1$
	ALERR CMNFRC		;Complain if any problems
1$:	MOV RF,SP		;Restore stack
	MOV -2(SP),RF		;RF ← old PC
	RTS RF			;Just return

CMGO:	MOV CMCB(R4),R0		;R0 ← LOC[c-m's control block]
	BIS #CMRUN,CMSTAT(R0)	;Set the run bit
	BIC #CMENB,CMSTAT(R0)	;Clear the enable bit
	MOV PDB(R4),R0		;R0 ← LOC[c-m's PDB]
	MOV USKMAX(R0),SP	;Reset stack pointer
	JMP INTERP		;Go interpret the c-m's body

COMPLY: ;Interpreter routine
COMMENT ⊗ Gets magnitude of force to apply (scalar on R3 stack) and the control bits via
(the arm and force component to apply) follow via the IPC. ⊗
	FETCH R0		;Get bits for COMPLY
	LDF @(R3)+,AC0		;Get the force value
	JSR PC,@LCOMPLY		;Set up the force to apply
	TST R0
	BEQ 1$
	ALERR CMNCMP		;Complain if any problems
1$:	CCC
	RTS PC			;Done - return

CMPOFF: ;Interpreter routine
	ALERR NOCMPF		;Complain - CMPOFF hasn't been written yet
	CCC
	RTS PC

CMSENSE: ALERR CMNOSE		;Aren't any of these guys yet
	MOV RF,SP		;Restore stack
	MOV -2(SP),RF		;RF ← old PC
	RTS RF			;Just return

DATA
CMNSET: ASCIE </COULDN'T INITIALIZE FORCE SYSTEM/>
CMNFRC: ASCIE </COULDN'T QUEUE FORCE CMON/>
CMNCMP: ASCIE </COULDN'T SET UP FORCE COMPLIANCE/>
NOCMPF: ASCIE </CAN'T TURN OFF COMPLIANCE YET/>
CMNOSE: ASCIE </HARDWARE MONITORING ISN'T READY YET/>
CODE
;Events:  SIGNAL, WAITE, PAUSE

COMMENT ⊗ Events can be created (at the beginnings of blocks is the
usual place), signaled, awaited (in the middle of a block) and
destroyed (at the end of a block).  Each event is a variable, that
is, it is refered to by a level-offset pair.  However, its place in
the environment does not point to a graph node, since there is no
such thing as attachment to an event.  The event itself is stored in
the environment.  The garbage collector marking phase had better
understand this.  ⊗

SIGNAL:	;Interpreter routine.  Signal the event of the level-offset pair.
	FETCH R0	;R0 ← level-offset pair.
	JSR PC,GETARG	;R0 ← equivalent pointer into environment
	EVSIG 2(R0)	;Signal that event.
	CCC		;Clear condition code.
	RTS PC		;Done

WAITE:	;Interpreter routine.  Wait on the event of the level-offset pair.
	FETCH R0	;R0 ← level-offset pair.
	JSR PC,GETARG	;R0 ← equivalent pointer into environment
	EVWAIT 2(R0)	;Wait on that event.
	BCC 1$		;Return OK?
	JMP TERMINATE	;The event was destroyed.  I guess we should depart cleanly.
1$:	JSR PC,NOTICE	;Assume the world has gone awry.
	CCC		;Clear condition code.
	RTS PC		;Done

PAUSE:	;Interpreter routine
COMMENT ⊗ Pause n seconds, where n is on the stack.  ⊗
	LDF @(R3)+,AC0	;AC0 ← wait time
	MULF THOUS,AC0	;AC0 ← time, in milliseconds
	STCFI AC0,R0	;R0 ← time in milliseconds
	SLEEP R0	;The pause that refreshes
	CCC		;Clear Condition code
	RTS PC		;Done

DATA
THOUS:	.FLT2 1000.0
CODE
;Output routines:  PRINT, VALPRN, VARPRN, TACKVAL, TYPVAL, CVFX

PRINT:	;Interpreter routine
	FETCH R0	;R0 ← Address of string
	EVWAIT CSLEVT
	JSR PC,TYPSTR	;Type it out
	EVSIG CSLEVT
	CCC		;Clear condition code
	RTS PC		;Done

VARPRN:	
COMMENT ⊗ Interpreter routine.  Prints the graph node pointed to by
the level-offset of the argument.  ⊗
	JSR PC,GTVAL	;Let GTVAL put value on stack
	JMP VALPRN	;And let VALPRN take it from there.

VALPRN:	
COMMENT ⊗ Interpreter routine.  Prints the value the top of the stack
and pops it.  ⊗
	MOV (R3)+,R0	;R0 ← LOC[value cell]
	JSR PC,TYPVAL	;Go print it.
	CCC		;Clear condition codes
	RTS PC		;And return

.IFNZ ALAID

TACKVAL:
COMMENT ⊗ R1 points to a value cell.  R0 points to a string where the
value is to be placed.  Places it there just as TYPVAL prints it out,
using common code.  ⊗
	PUSH <R2,R3,#TACKV>	;Save R2 & R3 & address of the placing routine.
	MOV R0,R2	;Exchange R0, R1
	MOV R1,R0
	MOV R2,R1	;Now R0 = value cell, R1 = string where to put it.
	JSR PC,TYPVL	;And do just as TYPVAL does.
	MOV R1,R0	;Put back the final string pointer
	BR TYPVRT	;Return

TACKV:	
COMMENT ⊗ R1 = string pointer, R0 = new addition.  Use TACK to put it
on. ⊗
	PUSH <R2>	;Save R2
	MOV R0,R2
	MOV R1,R0
	MOV R2,R1	;Now R0 = string poiter, R1 = new addition.
	JSR PC,TACK
	MOV R0,R1	;R1 = final string pointer.
	POP <R2>	;Restore R2
	RTS PC		;Done
.ENDC

TYPVAL:
COMMENT ⊗ R0 points to a value cell.  Prints it according to its
type.  Requires the floating package.  ⊗
	PUSH <R2,R3,#TYPSTR>	;Save R2 & R3 & address of the placing routine.
	EVWAIT CSLEVT
	JSR PC,TYPVL
	EVSIG CSLEVT
TYPVRT:	TST (SP)+	;Get rid of the address of typing routine.
	POP <R3,R2>	;Restore R3 & R2
	RTS PC

	;R0 = LOC[value cell], R1 = LOC[string] in some cases.
	;R2, R3 are available for use.

TYPVL:	MOV R0,R2		;R2 ← LOC[value cell]
	MOV #CRLFX,R0		;CRLF
	JSR PC,@2(SP)
	CMPB #SCLID,TAGID(R2)	;A scalar?
	BEQ 1$
	CMPB #VCTID,TAGID(R2)	;A vector?
	BEQ 4$
	CMPB #TRNID,TAGID(R2)	;A trans?
	BEQ 5$
1$:	MOV #SNAME,R0
	JSR PC,@2(SP)		;"SCALAR "
	MOV #OUTBUF,R0
2$:	LDF (R2),AC0
	JSR PC,CVFX
	MOV #OUTBUF,R0
	JSR PC,@2(SP)
3$:	MOV #CRLFX,R0		;CRLF
	JSR PC,@2(SP)
	RTS PC			;Done
4$:	MOV #VNAME,R0
	JSR PC,@2(SP)		;"VECTOR "
	MOV #OUTBUF,R0
	LDF (R2)+,AC0
	JSR PC,CVFX
	LDF (R2)+,AC0
	JSR PC,CVFX
	BR  2$			;Bum code for last field.
5$:	MOV #TNAME,R0
	JSR PC,@2(SP)		;"TRANS "
	PUSH <R3>		;Save R3
	MOV #3,R3		;R3 ← Number of rows
6$:	MOV #CRLFX,R0
	JSR PC,@4(SP)
	MOV #OUTBUF,R0
	LDF (R2),AC0
	JSR PC,CVFX
	LDF 14(R2),AC0
	JSR PC,CVFX
	LDF 30(R2),AC0
	JSR PC,CVFX
	LDF 44(R2),AC0
	JSR PC,CVFX
	MOV #OUTBUF,R0
	JSR PC,@4(SP)
	ADD #4,R2		;Next row
	SOB R3,6$
	MOV #CRLFX,R0
	JSR PC,@4(SP)
	MOV #OUTBUF,R0
	MOV #3,R3		;Now do the 0 0 0 1 row
7$:	CLRF AC0
	JSR PC,CVFX
	SOB R3,7$
	LDF ONE,AC0
	JSR PC,CVFX
	MOV #OUTBUF,R0
	JSR PC,@4(SP)
	POP <R3>		;Restore R3
	BR  3$			;Go to the exit stage

CVFX:	;Version of CVF that saves R1.
	PUSH <R1>
	JSR PC,CVF
	POP <R1>
	RTS PC

DATA
SNAME:	ASCIE /SCALAR /
VNAME:	ASCIE /VECTOR /
TNAME:	ASCIE /TRANS /
CODE
;  BREAK, NOOP, TOPAL

    .IFZ ALAID
BREAK:	;Interpreter routine
	MOV #BRKMES,R0
	JSR PC,TYPSTR
	BPT		;Cause a DDT break
	CCC		;Clear condition code
	RTS PC		;Done
DATA
BRKMES:	ASCIE </
PROGRAM BREAK/>
CODE
    .ENDC
	RTS PC		;Done

TOPAL:	;Interpreter routine
        COMMENT ⊗ Escape to PAL.  JSRs to the pseudo code.  That code
        should return via: 
            MOV PC,R0
            RTS PC
	⊗
	JSR PC,@IPC(R4)	;Fly
	ADD #2,R0	;R0 ← Proper new IPC
	MOV R0,IPC(R4)	;Hope R4, R3 still OK!
	RTS PC		;Done.

;Initialization psops:  PROG, ENDP, FIXIT (******* CHANGE THESE TOO ********)

PROG:
COMMENT ⊗  Zeros the calc fields for the arms & makes the main interpreter
environment.  ⊗
	JSR PC,NEWENV		;Create the main environment
	MOV #SYSENV,SLINK(R0)	;Set up the pointer to SYSENV
	CLR BARMHD+CALCS	;Kill any old calcs for BARM
	CLR YARMHD+CALCS	;Kill any old calcs for YARM
	CCC			;Clear condition code
	RTS PC			;Done

ENDP:
COMMENT ⊗ Releases main interpreter environment. ⊗
calculators.  This is done by using some special-purpose pseudo-code
	JMP TERMINATE		;Done with the interpreter

FIXIT:	
COMMENT ⊗ This should only have to be called from DDT.  Unwedges the
servos.  ⊗
	MOV #34,R0		;
	JSR PC,GTFREE		;Get a device block
	MOV R0,-(SP)		;
	MOV R0,R1		;
	JSR PC,@LINTARM		;Initialize all servos
	TST R0			;All well?
	BEQ 1$			;Yes
	MOV R0,-(SP)		;No
	MOV #FIXM,R0		;Complain.
	JSR PC,TYPSTR		;   without getting back into DDT prematurely
	MOV (SP)+,R0		;
	JSR PC,TYPOCT		;
1$:	MOV (SP)+,R0		;
	JSR PC,RLFREE		;Reclaim the device block
	RTS PC			;
DATA
FIXM:	ASCIE </
CAN'T INITIALIZE ARM.  ERROR CODE = />
CODE
;BUGS

COMMENT ⊗
Any variables (like FORCE variables, CMONS, or ordinary declared
variables) inside the conclusion of a CMON use level-offsets later
used for other things.  Fix: Let the conclusion of a CMON be at a new
lexical level.  Changes to PASS3: Trivial.  Changes to INTERP: Make
CMTRIG do the nasty work. (DONE changes made to PASS3 & CMMAK - ARG 11/76)
⊗